diff --git a/AGENTS.md b/AGENTS.md index b2c077191d5..f951d72bb23 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -327,6 +327,7 @@ docs — check it before assuming something isn't documented: versioning rules, when a new setting is warranted) - `build-system.md` - CMake build system internals - `pid-to-servo-computation.md` - PID-to-servo signal path (fixed-wing) +- `non-blocking-i2c.md` - Non-blocking I2C sensor access (bus API, baro/compass/pitot hooks, driver conversion) - `msp/` - MSP protocol doc generation tooling - Target-specific files in `/src/main/target/` diff --git a/docs/Settings.md b/docs/Settings.md index a0bdc187935..88df75b4845 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -803,6 +803,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) | ESC | | | FW_TURN | | | MAG | | +| I2C | | --- diff --git a/docs/development/non-blocking-i2c.md b/docs/development/non-blocking-i2c.md new file mode 100644 index 00000000000..2a551c1d8a5 --- /dev/null +++ b/docs/development/non-blocking-i2c.md @@ -0,0 +1,184 @@ +# Non-blocking I2C sensor access + +How the barometer, compass and pitot tasks read their sensors without +blocking the scheduler, and how to convert a driver. + +## Why + +An I2C register read of 6 bytes at 400 kHz keeps the wire busy for about +200 µs. Before this change every `busReadBuf()` spun in the platform driver +until the whole transfer had finished, so a BARO task that only needed a few +microseconds of CPU showed up as ~200 µs in `tasks`, and the compass, pitot +and rangefinder tasks behaved the same way. + +Now the sensor task starts a transfer, returns, and parses the result on a +later run. The wire time stays the same, the CPU time moves to a handful of +short interrupts. The model follows Betaflight: no bus queue, no DMA, every +driver owns one outstanding transfer and polls for its completion. + +## Layers + +### Platform drivers (`drivers/bus_i2c_*.c`) + +Every platform implements the same `drivers/bus_i2c.h` API: + +| Function | Behaviour | +|---|---| +| `i2cRead()`, `i2cWrite()`, `i2cWriteBuffer()` | Blocking, unchanged for callers. They first wait for a pending non-blocking transfer, so drivers that were not converted keep working on a shared bus. | +| `i2cReadStart()`, `i2cWriteStart()` | Start a transfer and return at once. `false` means nothing was started (bus busy or start failed), try again later. | +| `i2cBusy(device, addr, &error)` | `true` while the given slave's non-blocking transfer runs. `error` reports the outcome of that slave's last completed transfer; the result is kept per address, so another device using the bus in between does not overwrite it. A transfer that never completes is reset after `I2C_TIMEOUT` and reported as failed to its owner. | + +| Platform | Implementation | +|---|---| +| STM32F4 | Own interrupt-driven engine in `bus_i2c_stm32f40x.c`, ported from the Betaflight `bus_i2c_stm32f4xx.c` driver. Fully asynchronous. | +| STM32F7/H7 | `HAL_I2C_Mem_Read_IT()` and friends, completion through the `HAL_I2C_*CpltCallback()` / `HAL_I2C_ErrorCallback()` hooks. Fully asynchronous. | +| AT32F43x | `i2c_memory_read_int()` and friends from `i2c_application`. The library waits for the bus to be free and sends the slave and register address synchronously (tens of µs, bounded by the library timeout), only the data phase runs in the interrupt. | +| RP2350 | Commands are queued into the controller FIFO and finished by the `STOP_DET` / `TX_ABRT` interrupt, ported from the Betaflight Pico driver. Blocking calls still use the Pico SDK functions. | +| Software I2C, SITL | Synchronous: the start call completes the transfer, `i2cBusy()` is always `false`. | + +`i2cWriteStart()` copies the data byte into driver-owned storage, the caller +does not have to keep it alive. Buffers given to `i2cReadStart()` must stay +valid until `i2cBusy()` reports idle, so drivers use static buffers. + +### Bus layer (`drivers/bus.h`) + +| Function | Behaviour | +|---|---| +| `busReadBufStart(dev, reg, buf, len)` | Non-blocking register read. On SPI the read completes inside the call. | +| `busWriteStart(dev, reg, data)` | Non-blocking single register write. | +| `busWriteBufStart(dev, reg, buf, len)` | Non-blocking multi-byte write, the buffer must stay valid until the bus is idle. A zero length sends the register byte alone. | +| `busIsBusy(dev, &error)` | Poll for completion of this device's transfer: `true` while it is on the bus, otherwise `error` carries its outcome. Another device's transfer on the same bus does not affect the result. | +| `busReadStepResult_e` | `BUS_READ_STEP_BUSY` (nothing started), `BUS_READ_STEP_NEXT` (intermediate transfer started), `BUS_READ_STEP_LAST` (final transfer started). Used by sensors whose sample needs several transfers. | + +Raw access (`reg == 0xFF` with `DEVFLAGS_USE_RAW_REGISTERS`) works the same +way as for the blocking calls. + +### Sensor layers + +Each sensor subsystem drives its own small state machine and reschedules +its task with the returned delay. While a transfer is on the bus the task +asks to be called again after 1 ms, otherwise it returns to its regular +period. + +**Barometer** (`sensors/barometer.c`, hooks in `drivers/barometer/barometer.h`) + +``` +start_ut -> (ut_delay) -> read_ut -> get_ut -> start_up -> (up_delay) -> read_up -> get_up -> calculate +``` + +* `start_*` triggers a measurement, usually a `busWriteStart()`. +* `read_*` starts the data transfer with `busReadBufStart()` and returns + `false` when the bus is busy. +* `get_*` only parses the static buffer and returns `false` when the sample + is unusable (phase is restarted). +* `combined_read = true` skips the temperature phase for sensors that deliver + both values in one read. +* Drivers without `read_*` hooks keep their blocking `get_*` and run exactly + as before. + +A delay of 0 keeps the current task period. The estimator is fed only when a +new sample was produced. + +**Compass** (`sensors/compass.c`, hooks in `drivers/compass/compass.h`) + +`readStart(mag, firstStep)` returns a `busReadStepResult_e`. It is called with +`firstStep` set for the first transfer of a sample and again after every +completed transfer until it returns `BUS_READ_STEP_LAST`; `read()` then parses. +All I2C compass drivers are converted: one step for sensors in continuous +mode (HMC5883L, IST8308, IST8310, VCM5883, MLX90393), status then data for +sensors with a data-ready flag (QMC5883L/P, LIS2MDL, LIS3MDL, MAG3110, +RM3100), and status, data and the trigger of the next single measurement for +the AK8963 (three steps) and AK8975 (four steps, STATUS2 is read as well). + +**Pitot** (`sensors/pitotmeter.c`, hooks in `drivers/pitotmeter/pitotmeter.h`) + +Same `readStart(pitot, firstStep)` contract inside the existing protothread. +`start()` may be non-blocking as well. The MS4525 reads two frames per +sample (`NEXT`, `LAST`), MS5525 and DLVR one. + +**Rangefinder** (`sensors/rangefinder.c`, hooks in `drivers/rangefinder/rangefinder.h`) + +`update()` returns the delay until its next call, 0 meaning the regular +`delayMs`. Each I2C driver keeps a small step machine: it starts one +transfer per call, returns `RANGEFINDER_UPDATE_RETRY_US` while the transfer +is on the bus and evaluates the result on the next call. The VL53L0X start +sequence of eight register writes takes eight calls, the VL53L1X sends the +16 bit register index and reads the data in separate steps, the interrupt +polarity it needs is read once at init. + +**Others** + +* IRLock: `read()` returns `IRLOCK_READ_PENDING` while the sync scan or the + frame read is on the bus, `irlockUpdate()` reschedules the task. +* INA226: `ina226Update()` is polled from the battery task and alternates + bus and shunt voltage reads in the background; the read functions return + the last completed values, so each quantity refreshes every other battery + task cycle. +* LM75: `readStart()` plus `read()`, the protothread waits for the bus with + `ptWait()`. The result lands in the device scratchpad. +* DS2482 1-Wire bridge: commands are fired with `busWriteStart()` (blocking + fallback when the bus is busy), `owBusReady()` starts a status read on one + poll and evaluates it on the next. Reading the data register stays + blocking, two short transfers per byte. +* PCF8574: the output write is fired and forgotten, with the blocking write + as fallback. +* OLED (`display_ug2864hsweg01.c`): the drawing calls write into a shadow + of the panel's GDDRAM (8 pages x 128 columns) and never wait for the bus. + `i2c_OLED_pump()` streams the columns that changed: a 3 byte page/column + address run, then data runs of up to 15 bytes, one transfer per call. The + dashboard task calls the pump every millisecond while something is dirty, + the CMS display port reports that through `isTransferInProgress`. A + column rewritten with the same value costs nothing, a chunk lost to a bus + error is marked dirty again and the pump backs off for 200 ms. Only the + initialisation (command table, first clear) is blocking. + +## Converting a driver + +1. Replace every bus access in the measurement path with a static buffer + and a `busReadBufStart()` / `busWriteStart()` call that returns the + `BUSY` / started result to the sensor layer. +2. Keep `get()` / `read()` free of bus access: parse the buffer, validate, + return `false` for an unusable sample. +3. Leave initialisation and detection on the blocking calls. +4. If the sample needs several transfers, keep the step counter in the + driver, reset it on `firstStep`, and return `NEXT` until the last transfer. +5. Check the cadence: forced-mode sensors now sample every + `delay + 1 ms` instead of `2 * delay`, because the next measurement is + triggered right after the previous one was read. + +Drivers for other buses (SPI gyros, MSP, fake sensors) need no change. + +Every driver that was converted keeps its blocking init path; only the +periodic measurement path changed. + +## Debugging + +`set debug_mode = I2C`, then `debug` in the CLI: + +| Field | Meaning | +|---|---| +| `debug[0]` | interrupts during the last transfer (about 10 for a 6 byte read on F4) | +| `debug[1]` | error interrupts since boot | +| `debug[2]` | duration of the last transfer in µs (about 200 for 6 bytes at 400 kHz) | +| `debug[3]` | longest single interrupt handler run in µs (F4 only) | +| `debug[4]` | longest wait for a START / STOP bit release in µs (F4 only) | +| `debug[5]` | I2C error counter, also shown by `status` | +| `debug[6]` | interrupt enables found at transfer start (F4 only, 1024 is normal) | +| `debug[7]` | time spent in the start call in µs, should be a few µs on F4/F7/H7/RP2350 | + +In `tasks` the BARO, COMPASS and PITOT rates alternate between the regular +rate and 1000 Hz, because a cycle consists of a long wait and a 1 ms step. +The average execution time is what matters. + +## Known limitations + +* No bus arbitration queue: a device that finds the bus busy retries 1 ms + later. Fine at the 10 to 60 Hz rates these sensors use. +* Initialisation and detection of every driver, the DS2482 data register + read and the VL53L1X register helpers used at init still block. They are + safe because the blocking calls wait for a pending transfer first. +* The MPU9250 internal compass is read through the gyro's SPI slave interface + and keeps its own polling state machine. +* On the RP2350 a non-blocking write must fit the controller FIFO: the + register byte plus 15 data bytes. +* SPI stays synchronous. diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 1cfd028c3f9..a55102305de 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -123,8 +123,6 @@ main_sources(COMMON_SRC drivers/barometer/barometer_lps25h.h drivers/barometer/barometer_ms56xx.c drivers/barometer/barometer_ms56xx.h - drivers/barometer/barometer_spl06.c - drivers/barometer/barometer_spl06.h drivers/barometer/barometer_msp.c drivers/barometer/barometer_msp.h drivers/barometer/barometer_crsf.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 23caa6b6fa7..4b10d82202e 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -87,6 +87,7 @@ typedef enum { DEBUG_ESC, DEBUG_FW_TURN, DEBUG_MAG, + DEBUG_I2C, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/drivers/1-wire/ds2482.c b/src/main/drivers/1-wire/ds2482.c index cfe10b4d5cc..ed4837142ee 100644 --- a/src/main/drivers/1-wire/ds2482.c +++ b/src/main/drivers/1-wire/ds2482.c @@ -82,6 +82,13 @@ static bool ds2482Reset(owDev_t *owDev) return busWrite(owDev->busDev, 0xFF, DS2482_RESET_CMD); } +// 1-Wire commands are fired without waiting for the bus; the blocking write is the fallback when another +// device's transfer is in flight, so a command is never lost +static bool ds2482WriteCommand(owDev_t *owDev, uint8_t reg, uint8_t data) +{ + return busWriteStart(owDev->busDev, reg, data) || busWrite(owDev->busDev, reg, data); +} + static bool ds2482SetReadPtr(owDev_t *owDev, uint8_t reg) { return busWrite(owDev->busDev, DS2482_SET_READ_PTR_CMD, reg); @@ -137,16 +144,27 @@ static bool ds2482WaitForBus(owDev_t *owDev) return ds2482Poll(owDev, true, NULL); } +// Polled from the temperature protothread: a status read is started on one call and evaluated on a later one static bool ds2482OwBusReady(owDev_t *owDev) { - bool ack = busRead(owDev->busDev, 0xFF, &owDev->status); - if (!ack) return false; - return !DS2482_1WIRE_BUSY(owDev->status); + static bool statusReadPending = false; + bool busError = false; + + if (statusReadPending) { + if (busIsBusy(owDev->busDev, &busError) && !busError) return false; + statusReadPending = false; + if (busError) return false; + return !DS2482_1WIRE_BUSY(owDev->status); + } + + // after a command the read pointer sits on the status register + statusReadPending = busReadBufStart(owDev->busDev, 0xFF, &owDev->status, 1); + return false; } static bool ds2482OwResetCommand(owDev_t *owDev) { - return busWrite(owDev->busDev, 0xFF, DS2482_1WIRE_RESET_CMD); + return ds2482WriteCommand(owDev, 0xFF, DS2482_1WIRE_RESET_CMD); } static bool ds2482OwReset(owDev_t *owDev) @@ -158,7 +176,7 @@ static bool ds2482OwReset(owDev_t *owDev) static bool ds2482OwWriteByteCommand(owDev_t *owDev, uint8_t byte) { - return busWrite(owDev->busDev, DS2482_1WIRE_WRITE_BYTE_CMD, byte); + return ds2482WriteCommand(owDev, DS2482_1WIRE_WRITE_BYTE_CMD, byte); } static bool ds2482OwWriteByte(owDev_t *owDev, uint8_t byte) @@ -179,7 +197,7 @@ static bool ds2482OwWriteBuf(owDev_t *owDev, const uint8_t *buf, uint8_t len) static bool ds2482OwReadByteCommand(owDev_t *owDev) { - return busWrite(owDev->busDev, 0xFF, DS2482_1WIRE_READ_BYTE_CMD); + return ds2482WriteCommand(owDev, 0xFF, DS2482_1WIRE_READ_BYTE_CMD); } static bool ds2482OwReadByte(owDev_t *owDev, uint8_t *result) @@ -204,7 +222,7 @@ static bool ds2482OwReadBuf(owDev_t *owDev, uint8_t *buf, uint8_t len) static bool ds2482OwSingleBitCommand(owDev_t *owDev, uint8_t type) { - return busWrite(owDev->busDev, DS2482_1WIRE_SINGLE_BIT_CMD, type); + return ds2482WriteCommand(owDev, DS2482_1WIRE_SINGLE_BIT_CMD, type); } static bool ds2482OwSingleBitResult(owDev_t *owDev) @@ -225,7 +243,7 @@ static bool ds2482OwSingleBit(owDev_t *owDev, uint8_t type, bool *result) static bool ds2482OwTripletCommand(owDev_t *owDev, uint8_t direction) { - return busWrite(owDev->busDev, DS2482_1WIRE_TRIPLET_CMD, direction << 7); + return ds2482WriteCommand(owDev, DS2482_1WIRE_TRIPLET_CMD, direction << 7); } static uint8_t ds2482OwTripletResult(owDev_t *owDev) diff --git a/src/main/drivers/barometer/barometer.h b/src/main/drivers/barometer/barometer.h index 980963f3f1f..cb7c3ad63e7 100644 --- a/src/main/drivers/barometer/barometer.h +++ b/src/main/drivers/barometer/barometer.h @@ -23,13 +23,24 @@ struct baroDev_s; typedef bool (*baroOpFuncPtr)(struct baroDev_s * baro); typedef bool (*baroCalculateFuncPtr)(struct baroDev_s * baro, int32_t *pressure, int32_t *temperature); +/* + * Measurement cycle driven by baroUpdate(): start_ut -> (ut_delay) -> read_ut -> get_ut -> start_up -> (up_delay) -> read_up -> get_up -> calculate + * + * A driver may leave any hook NULL. The optional read_* hooks start a non-blocking bus transfer and must return false + * when the bus is busy; the matching get_* then only parses the received buffer once the bus reports idle and returns + * false if the sample is not usable (the phase is restarted). Drivers without read_* hooks do their blocking bus access + * inside get_*. A delay of 0 keeps the current task period. + */ typedef struct baroDev_s { busDevice_t * busDev; + bool combined_read; // get_up delivers temperature as well, the temperature phase is skipped uint16_t ut_delay; uint16_t up_delay; baroOpFuncPtr start_ut; + baroOpFuncPtr read_ut; baroOpFuncPtr get_ut; baroOpFuncPtr start_up; + baroOpFuncPtr read_up; baroOpFuncPtr get_up; baroCalculateFuncPtr calculate; } baroDev_t; diff --git a/src/main/drivers/barometer/barometer_2smpb_02b.c b/src/main/drivers/barometer/barometer_2smpb_02b.c index 833f4a1ebc6..5755d477829 100644 --- a/src/main/drivers/barometer/barometer_2smpb_02b.c +++ b/src/main/drivers/barometer/barometer_2smpb_02b.c @@ -25,6 +25,7 @@ #include #include #include +#include #include @@ -169,7 +170,13 @@ static bool deviceDetect(busDevice_t * busDev) static bool b2smpbStartUP(baroDev_t *baro) { // start a forced measurement - return busWrite(baro->busDev, REG_CTRL_MEAS, REG_CLT_MEAS_VAL_TAVG4X_PAVG32X_FORCED); + return busWriteStart(baro->busDev, REG_CTRL_MEAS, REG_CLT_MEAS_VAL_TAVG4X_PAVG32X_FORCED); +} + +static bool b2smpbReadUP(baroDev_t *baro) +{ + // kick off the read of pressure and temperature, b2smpbGetUP() parses it once the bus is idle + return busReadBufStart(baro->busDev, REG_PRESS_TXD2, &baroDataBuf[0], 6); } static bool b2smpbGetUP(baroDev_t *baro) @@ -177,9 +184,7 @@ static bool b2smpbGetUP(baroDev_t *baro) int32_t dtp; float tr, pl, tmp; - if (!busReadBuf(baro->busDev, REG_PRESS_TXD2, &baroDataBuf[0], 6)) { - return false; - } + UNUSED(baro); // Calculate compensated temperature dtp = getSigned24bitValue(&baroDataBuf[3]); @@ -234,12 +239,17 @@ bool baro2SMPB02BDetect(baroDev_t *baro) return false; } + // Forced mode: one measurement per cycle, pressure and temperature come out of one read + baro->combined_read = true; + baro->up_delay = 35000; // measurement takes 33.7 ms with 4x / 32x averaging baro->start_up = b2smpbStartUP; + baro->read_up = b2smpbReadUP; baro->get_up = b2smpbGetUP; baro->ut_delay = 0; baro->start_ut = NULL; + baro->read_ut = NULL; baro->get_ut = NULL; baro->calculate = deviceCalculate; diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c index a60a64e0d7c..aabc7392207 100644 --- a/src/main/drivers/barometer/barometer_bmp085.c +++ b/src/main/drivers/barometer/barometer_bmp085.c @@ -100,6 +100,9 @@ STATIC_UNIT_TESTED bmp085_t bmp085; STATIC_UNIT_TESTED uint16_t bmp085_ut; // static result of temperature measurement STATIC_UNIT_TESTED uint32_t bmp085_up; // static result of pressure measurement +// Target of the non-blocking ADC read, filled by the bus driver in the background +static uint8_t bmp085_adc_buf[3]; + static void bmp085_get_cal_param(baroDev_t *baro) { uint8_t data[BMP085_PROM_DATA__LEN]; @@ -178,26 +181,33 @@ static int32_t bmp085_get_pressure(uint32_t up) static bool bmp085_start_ut(baroDev_t *baro) { - bool ack = busWrite(baro->busDev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); - return ack; + return busWriteStart(baro->busDev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); +} + +static bool bmp085_read_ut(baroDev_t *baro) +{ + // kick off the ADC read, bmp085_get_ut() parses it once the bus is idle + return busReadBufStart(baro->busDev, BMP085_ADC_OUT_MSB_REG, bmp085_adc_buf, 2); } static bool bmp085_get_ut(baroDev_t *baro) { - uint8_t data[2]; - bool ack = busReadBuf(baro->busDev, BMP085_ADC_OUT_MSB_REG, data, 2); - if (ack) { - bmp085_ut = (data[0] << 8) | data[1]; - } + UNUSED(baro); - return ack; + bmp085_ut = (bmp085_adc_buf[0] << 8) | bmp085_adc_buf[1]; + return true; } static bool bmp085_start_up(baroDev_t *baro) { uint8_t ctrl_reg_data = BMP085_P_MEASURE + (bmp085.oversampling_setting << 6); - bool ack = busWrite(baro->busDev, BMP085_CTRL_MEAS_REG, ctrl_reg_data); - return ack; + return busWriteStart(baro->busDev, BMP085_CTRL_MEAS_REG, ctrl_reg_data); +} + +static bool bmp085_read_up(baroDev_t *baro) +{ + // kick off the ADC read, bmp085_get_up() parses it once the bus is idle + return busReadBufStart(baro->busDev, BMP085_ADC_OUT_MSB_REG, bmp085_adc_buf, 3); } /** read out up for pressure conversion @@ -206,14 +216,10 @@ static bool bmp085_start_up(baroDev_t *baro) */ static bool bmp085_get_up(baroDev_t *baro) { - uint8_t data[3]; - bool ack = busReadBuf(baro->busDev, BMP085_ADC_OUT_MSB_REG, data, 3); - - if (ack) { - bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting); - } + UNUSED(baro); - return ack; + bmp085_up = (((uint32_t) bmp085_adc_buf[0] << 16) | ((uint32_t) bmp085_adc_buf[1] << 8) | (uint32_t) bmp085_adc_buf[2]) >> (8 - bmp085.oversampling_setting); + return true; } STATIC_UNIT_TESTED bool bmp085_calculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature) @@ -265,10 +271,12 @@ bool bmp085Detect(baroDev_t *baro) baro->ut_delay = UT_DELAY; baro->start_ut = bmp085_start_ut; + baro->read_ut = bmp085_read_ut; baro->get_ut = bmp085_get_ut; baro->up_delay = UP_DELAY; baro->start_up = bmp085_start_up; + baro->read_up = bmp085_read_up; baro->get_up = bmp085_get_up; baro->calculate = bmp085_calculate; diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c index 038c5029ce4..5124735d3d3 100644 --- a/src/main/drivers/barometer/barometer_bmp280.c +++ b/src/main/drivers/barometer/barometer_bmp280.c @@ -54,51 +54,30 @@ STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal; int32_t bmp280_up = 0; int32_t bmp280_ut = 0; -static bool bmp280_start_ut(baroDev_t * baro) -{ - UNUSED(baro); - return true; -} - -static bool bmp280_get_ut(baroDev_t * baro) -{ - UNUSED(baro); - return true; -} +// Target of the non-blocking data read, filled by the bus driver in the background +static uint8_t bmp280_data[BMP280_DATA_FRAME_SIZE]; static bool bmp280_start_up(baroDev_t * baro) { // start measurement // set oversampling + power mode (forced), and start sampling - busWrite(baro->busDev, BMP280_CTRL_MEAS_REG, BMP280_MODE); - return true; + return busWriteStart(baro->busDev, BMP280_CTRL_MEAS_REG, BMP280_MODE); } -static bool bmp280_get_up(baroDev_t * baro) +static bool bmp280_read_up(baroDev_t * baro) { - uint8_t data[BMP280_DATA_FRAME_SIZE]; - - //error free measurements - static int32_t bmp280_up_valid; - static int32_t bmp280_ut_valid; + // kick off the read of pressure and temperature, bmp280_get_up() parses it once the bus is idle + return busReadBufStart(baro->busDev, BMP280_PRESSURE_MSB_REG, bmp280_data, BMP280_DATA_FRAME_SIZE); +} - //read data from sensor - bool ack = busReadBuf(baro->busDev, BMP280_PRESSURE_MSB_REG, data, BMP280_DATA_FRAME_SIZE); +static bool bmp280_get_up(baroDev_t * baro) +{ + UNUSED(baro); - //check if pressure and temperature readings are valid, otherwise use previous measurements from the moment - if (ack) { - bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4)); - bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4)); - bmp280_up_valid = bmp280_up; - bmp280_ut_valid = bmp280_ut; - } - else { - //assign previous valid measurements - bmp280_up = bmp280_up_valid; - bmp280_ut = bmp280_ut_valid; - } + bmp280_up = (int32_t)((((uint32_t)(bmp280_data[0])) << 12) | (((uint32_t)(bmp280_data[1])) << 4) | ((uint32_t)bmp280_data[2] >> 4)); + bmp280_ut = (int32_t)((((uint32_t)(bmp280_data[3])) << 12) | (((uint32_t)(bmp280_data[4])) << 4) | ((uint32_t)bmp280_data[5] >> 4)); - return ack; + return true; } // Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC @@ -197,12 +176,17 @@ bool bmp280Detect(baroDev_t *baro) // set oversampling + power mode (forced), and start sampling busWrite(baro->busDev, BMP280_CTRL_MEAS_REG, BMP280_MODE); + // Forced mode: one measurement per cycle, pressure and temperature come out of one read + baro->combined_read = true; + baro->ut_delay = 0; - baro->get_ut = bmp280_get_ut; - baro->start_ut = bmp280_start_ut; + baro->start_ut = NULL; + baro->read_ut = NULL; + baro->get_ut = NULL; baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << BMP280_TEMPERATURE_OSR) >> 1) + ((1 << BMP280_PRESSURE_OSR) >> 1)) + (BMP280_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000; baro->start_up = bmp280_start_up; + baro->read_up = bmp280_read_up; baro->get_up = bmp280_get_up; baro->calculate = bmp280_calculate; diff --git a/src/main/drivers/barometer/barometer_bmp388.c b/src/main/drivers/barometer/barometer_bmp388.c index 1d5a8ad6e4d..00c333f8261 100644 --- a/src/main/drivers/barometer/barometer_bmp388.c +++ b/src/main/drivers/barometer/barometer_bmp388.c @@ -164,13 +164,15 @@ static uint8_t sensor_data[BMP388_DATA_FRAME_SIZE+1]; static int64_t t_lin = 0; -static bool bmp388StartUT(baroDev_t *baro); -static bool bmp388GetUT(baroDev_t *baro); static bool bmp388StartUP(baroDev_t *baro); +static bool bmp388ReadUP(baroDev_t *baro); static bool bmp388GetUP(baroDev_t *baro); static bool bmp388Calculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature); +// PWR_CTRL: pressure and temperature enabled, forced mode +#define BMP388_PWR_CTRL_FORCED_MEASUREMENT (BMP388_MODE_FORCED << 4 | 1 << 1 | 1 << 0) + static bool bmp388BeginForcedMeasurement(busDevice_t *busdev) { // enable pressure measurement, temperature measurement, set power mode and start sampling @@ -178,35 +180,27 @@ static bool bmp388BeginForcedMeasurement(busDevice_t *busdev) return busWrite(busdev, BMP388_PWR_CTRL_REG, mode); } -static bool bmp388StartUT(baroDev_t *baro) -{ - UNUSED(baro); - // dummy - return true; -} - -static bool bmp388GetUT(baroDev_t *baro) -{ - UNUSED(baro); - // dummy - return true; -} - static bool bmp388StartUP(baroDev_t *baro) { // start measurement - return bmp388BeginForcedMeasurement(baro->busDev); + return busWriteStart(baro->busDev, BMP388_PWR_CTRL_REG, BMP388_PWR_CTRL_FORCED_MEASUREMENT); } -static bool bmp388GetUP(baroDev_t *baro) +static bool bmp388ReadUP(baroDev_t *baro) { + // kick off the read of pressure and temperature, bmp388GetUP() parses it once the bus is idle if (baro->busDev->busType == BUSTYPE_SPI) { - // In SPI mode, first byte read is a dummy byte - busReadBuf(baro->busDev, BMP388_DATA_0_REG, &sensor_data[0], BMP388_DATA_FRAME_SIZE + 1); - } else { - // In I2C mode, no dummy byte is read - busReadBuf(baro->busDev, BMP388_DATA_0_REG, &sensor_data[1], BMP388_DATA_FRAME_SIZE); + // In SPI mode there is a dummy byte before the data + return busReadBufStart(baro->busDev, BMP388_DATA_0_REG, &sensor_data[0], BMP388_DATA_FRAME_SIZE + 1); + } + else { + return busReadBufStart(baro->busDev, BMP388_DATA_0_REG, &sensor_data[1], BMP388_DATA_FRAME_SIZE); } +} + +static bool bmp388GetUP(baroDev_t *baro) +{ + UNUSED(baro); bmp388_up = sensor_data[1] << 0 | sensor_data[2] << 8 | sensor_data[3] << 16; bmp388_ut = sensor_data[4] << 0 | sensor_data[5] << 8 | sensor_data[6] << 16; @@ -358,12 +352,17 @@ bool bmp388Detect(baroDev_t *baro) bmp388BeginForcedMeasurement(baro->busDev); + // Forced mode: one measurement per cycle, pressure and temperature come out of one read + baro->combined_read = true; + baro->ut_delay = 0; - baro->get_ut = bmp388GetUT; - baro->start_ut = bmp388StartUT; + baro->start_ut = NULL; + baro->read_ut = NULL; + baro->get_ut = NULL; baro->up_delay = 234 + (392 + ((1 << (BMP388_PRESSURE_OSR + 1)) * 2000)) + (313 + ((1 << (BMP388_TEMPERATURE_OSR + 1)) * 2000)); baro->start_up = bmp388StartUP; + baro->read_up = bmp388ReadUP; baro->get_up = bmp388GetUP; baro->calculate = bmp388Calculate; diff --git a/src/main/drivers/barometer/barometer_dps310.c b/src/main/drivers/barometer/barometer_dps310.c index 7f5607f95ad..d103fd61679 100644 --- a/src/main/drivers/barometer/barometer_dps310.c +++ b/src/main/drivers/barometer/barometer_dps310.c @@ -39,8 +39,10 @@ #include "drivers/barometer/barometer_dps310.h" // See datasheet at https://www.infineon.com/dgdl/Infineon-DPS310-DataSheet-v01_02-EN.pdf?fileId=5546d462576f34750157750826c42242 +// The Goertek SPL06-001 is register compatible with the DPS310 and reports the same chip ID, so this driver serves both. +// The SPL07-003 differs only in two extra calibration coefficients. -#if defined(USE_BARO) && defined(USE_BARO_DPS310) +#if defined(USE_BARO) && (defined(USE_BARO_DPS310) || defined(USE_BARO_SPL06)) #define DPS310_REG_PSR_B2 0x00 #define DPS310_REG_PSR_B1 0x01 @@ -110,6 +112,10 @@ typedef struct { static baroState_t baroState; static uint8_t chipId[1]; +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t measurementStatus; // MEAS_CFG, polled for PRS_RDY before the data is fetched +static uint8_t measurementBuf[6]; + // Helper functions static uint8_t registerRead(busDevice_t * busDev, uint8_t reg) @@ -252,26 +258,37 @@ static bool deviceConfigure(busDevice_t * busDev) return true; } +// The temperature phase of the measurement cycle is used to poll MEAS_CFG, the pressure phase fetches the data +static bool deviceStartReadStatus(baroDev_t *baro) +{ + return busReadBufStart(baro->busDev, DPS310_REG_MEAS_CFG, &measurementStatus, sizeof(measurementStatus)); +} + +static bool deviceCheckStatus(baroDev_t *baro) +{ + UNUSED(baro); + + // No new pressure result yet, the cycle restarts after ut_delay + return (measurementStatus & DPS310_MEAS_CFG_PRS_RDY) != 0; +} + +static bool deviceStartReadMeasurement(baroDev_t *baro) +{ + // Kick off the read of PSR_B2, PSR_B1, PSR_B0, TMP_B2, TMP_B1, TMP_B0 + return busReadBufStart(baro->busDev, DPS310_REG_PSR_B2, measurementBuf, sizeof(measurementBuf)); +} + static bool deviceReadMeasurement(baroDev_t *baro) { - // 1. Check if pressure is ready - bool pressure_ready = registerRead(baro->busDev, DPS310_REG_MEAS_CFG) & DPS310_MEAS_CFG_PRS_RDY; - if (!pressure_ready) { - return false; - } + UNUSED(baro); - // 2. Choose scaling factors kT (for temperature) and kP (for pressure) based on the chosen precision rate. + // 1. Choose scaling factors kT (for temperature) and kP (for pressure) based on the chosen precision rate. // The scaling factors are listed in Table 9. static float kT = 253952; // 16 times (Standard) static float kP = 253952; // 16 times (Standard) - // 3. Read the pressure and temperature result from the registers - // Read PSR_B2, PSR_B1, PSR_B0, TMP_B2, TMP_B1, TMP_B0 - uint8_t buf[6]; - if (!busReadBuf(baro->busDev, DPS310_REG_PSR_B2, buf, 6)) { - return false; - } - + // 2. Pick the pressure and temperature result from the buffer filled by deviceStartReadMeasurement() + const uint8_t *buf = measurementBuf; const int32_t Praw = getTwosComplement((buf[0] << 16) + (buf[1] << 8) + buf[2], 24); const int32_t Traw = getTwosComplement((buf[3] << 16) + (buf[4] << 8) + buf[5], 24); @@ -335,25 +352,27 @@ static bool deviceDetect(busDevice_t * busDev) return ack && (chipId[0] == DPS310_ID_REV_AND_PROD_ID || chipId[0] == SPL07_003_CHIP_ID); } -bool baroDPS310Detect(baroDev_t *baro) +// Probes devHwCount consecutive bus device slots starting at firstDevHw and keeps the first chip that answers +static bool baroDetectWithDevices(baroDev_t *baro, devHardwareType_e firstDevHw, int devHwCount) { busDevice_t * candidates[DPS310_ADDRESS_COUNT]; int candidateCount = 0; - for (int index = 0; index < DPS310_ADDRESS_COUNT; index++) { - candidates[index] = busDeviceInit(BUSTYPE_ANY, DEVHW_DPS310_0 + index, 0, OWNER_BARO); + for (int index = 0; index < devHwCount; index++) { + candidates[index] = busDeviceInit(BUSTYPE_ANY, firstDevHw + index, 0, OWNER_BARO); if (candidates[index]) { + busSetSpeed(candidates[index], BUS_SPEED_STANDARD); // both chips top out at 10 MHz on SPI, no-op on I2C candidateCount++; } } baro->busDev = NULL; - // Both addresses share one retry window: a late chip keeps the full window and 0x77 adds no boot delay + // All addresses share one retry window: a late chip keeps the full window and 0x77 adds no boot delay for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT && candidateCount > 0 && !baro->busDev; retry++) { delay(100); - for (int index = 0; index < DPS310_ADDRESS_COUNT && !baro->busDev; index++) { + for (int index = 0; index < devHwCount && !baro->busDev; index++) { if (!candidates[index] || !deviceDetect(candidates[index])) { continue; } @@ -368,7 +387,7 @@ bool baroDPS310Detect(baroDev_t *baro) } } - for (int index = 0; index < DPS310_ADDRESS_COUNT; index++) { + for (int index = 0; index < devHwCount; index++) { if (candidates[index] && candidates[index] != baro->busDev) { busDeviceDeInit(candidates[index]); } @@ -380,12 +399,18 @@ bool baroDPS310Detect(baroDev_t *baro) const uint32_t baroDelay = 1000000 / 32 / 2; // twice the sample rate to capture all new data - baro->ut_delay = 0; + // Continuous mode, nothing needs to be triggered. The temperature phase polls PRS_RDY every baroDelay, + // the pressure phase reads the result right after the flag was seen. + baro->combined_read = false; + + baro->ut_delay = baroDelay; baro->start_ut = NULL; - baro->get_ut = NULL; + baro->read_ut = deviceStartReadStatus; + baro->get_ut = deviceCheckStatus; - baro->up_delay = baroDelay; + baro->up_delay = 1000; // one scheduler step between the status check and the data read baro->start_up = NULL; + baro->read_up = deviceStartReadMeasurement; baro->get_up = deviceReadMeasurement; baro->calculate = deviceCalculate; @@ -393,4 +418,19 @@ bool baroDPS310Detect(baroDev_t *baro) return true; } +#if defined(USE_BARO_DPS310) +bool baroDPS310Detect(baroDev_t *baro) +{ + // SDO low selects 0x76, SDO high 0x77; both slots are registered, the first one that answers wins + return baroDetectWithDevices(baro, DEVHW_DPS310_0, DPS310_ADDRESS_COUNT); +} +#endif + +#if defined(USE_BARO_SPL06) +bool baroSPL06Detect(baroDev_t *baro) +{ + return baroDetectWithDevices(baro, DEVHW_SPL06, 1); +} +#endif + #endif diff --git a/src/main/drivers/barometer/barometer_dps310.h b/src/main/drivers/barometer/barometer_dps310.h index 28ec6d59dd0..dba89e2a640 100644 --- a/src/main/drivers/barometer/barometer_dps310.h +++ b/src/main/drivers/barometer/barometer_dps310.h @@ -25,3 +25,4 @@ #pragma once bool baroDPS310Detect(baroDev_t *baro); +bool baroSPL06Detect(baroDev_t *baro); // SPL06-001 handled by the DPS310 driver diff --git a/src/main/drivers/barometer/barometer_lps25h.c b/src/main/drivers/barometer/barometer_lps25h.c index 5ea6bbb0a91..ec5b9a5a61c 100644 --- a/src/main/drivers/barometer/barometer_lps25h.c +++ b/src/main/drivers/barometer/barometer_lps25h.c @@ -103,6 +103,11 @@ static int32_t pressureRaw; static int32_t temperatureRaw; +// STATUS_REG, PRESS_OUT_XL/L/H and TEMP_OUT_L/H are consecutive, one burst read brings the ready flags and both results. +// Filled by the bus driver in the background. +#define LPS25HB_BURST_LEN 6 +static uint8_t lps25h_data[LPS25HB_BURST_LEN]; + static bool lps25hbRebootCmd(baroDev_t * baro) { uint8_t tmpreg; @@ -211,63 +216,29 @@ static bool lps25hbInit(baroDev_t * baro) return true; } - // check the status register bit to be present -static bool isDataReady(baroDev_t * baro, uint8_t bitMask) -{ - uint8_t tmp = 0; - - bool ack = busRead(baro->busDev, LPS25HB_STATUS_REG_ADDR, &tmp); - if (!ack) { - return false; - } - return (tmp & bitMask); -} - -static bool lps25h_start_up(baroDev_t * baro) +static bool lps25h_read_up(baroDev_t * baro) { - UNUSED(baro); - return true; + // kick off the burst read of status, pressure and temperature, lps25h_get_up() parses it once the bus is idle + const uint8_t autoIncrement = (baro->busDev->busType == BUSTYPE_SPI) ? LPS25HB_SPI_MULTIPLEBYTE_CMD : LPS25HB_I2C_MULTIPLEBYTE_CMD; + return busReadBufStart(baro->busDev, autoIncrement | LPS25HB_STATUS_REG_ADDR, lps25h_data, LPS25HB_BURST_LEN); } static bool lps25h_get_up(baroDev_t * baro) { - uint8_t buffer[3] = {0}; + UNUSED(baro); - if (isDataReady(baro, PRESSURE_READY_MASK)) { - if (baro->busDev->busType == BUSTYPE_SPI) { - busReadBuf(baro->busDev, LPS25HB_SPI_MULTIPLEBYTE_CMD | LPS25HB_PRESS_POUT_XL_ADDR, buffer, sizeof(buffer)); - } - else { - busReadBuf(baro->busDev, LPS25HB_I2C_MULTIPLEBYTE_CMD | LPS25HB_PRESS_POUT_XL_ADDR, buffer, sizeof(buffer)); - } + const uint8_t status = lps25h_data[0]; - pressureRaw = (int32_t)((uint32_t)buffer[2] << 16 | (uint32_t)buffer[1] << 8 | (uint32_t)buffer[0]); + if (!(status & PRESSURE_READY_MASK)) { + return false; // no new pressure sample yet, come back after up_delay } - return true; -} - -static bool lps25h_start_ut(baroDev_t * baro) -{ - UNUSED(baro); - return true; -} + pressureRaw = (int32_t)((uint32_t)lps25h_data[3] << 16 | (uint32_t)lps25h_data[2] << 8 | (uint32_t)lps25h_data[1]); -static bool lps25h_get_ut(baroDev_t * baro) -{ - uint8_t data[2] = {0}; - - if (isDataReady(baro, TEMPERATURE_READY_MASK)) { - if (baro->busDev->busType == BUSTYPE_SPI) { - busReadBuf(baro->busDev, LPS25HB_SPI_MULTIPLEBYTE_CMD | LPS25HB_TEMP_OUT_L_ADDR, data, sizeof(data)); - } - else { - busReadBuf(baro->busDev, LPS25HB_I2C_MULTIPLEBYTE_CMD | LPS25HB_TEMP_OUT_L_ADDR, data, sizeof(data)); - } - - temperatureRaw = (int16_t)((uint16_t)data[1] << 8 | (uint16_t)data[0]); + if (status & TEMPERATURE_READY_MASK) { + temperatureRaw = (int16_t)((uint16_t)lps25h_data[5] << 8 | (uint16_t)lps25h_data[4]); } return true; @@ -326,12 +297,17 @@ bool lps25hDetect(baroDev_t *baro) lps25hbInit(baro); + // Continuous mode at 25 Hz, pressure and temperature come out of one burst read + baro->combined_read = true; + baro->ut_delay = 0; - baro->get_ut = lps25h_get_ut; - baro->start_ut = lps25h_start_ut; + baro->start_ut = NULL; + baro->read_ut = NULL; + baro->get_ut = NULL; - baro->up_delay = 1000; - baro->start_up = lps25h_start_up; + baro->up_delay = 20000; // twice the output data rate to catch every sample + baro->start_up = NULL; + baro->read_up = lps25h_read_up; baro->get_up = lps25h_get_up; baro->calculate = lps25h_calculate; diff --git a/src/main/drivers/barometer/barometer_ms56xx.c b/src/main/drivers/barometer/barometer_ms56xx.c index a911c545d15..5647f7bc691 100644 --- a/src/main/drivers/barometer/barometer_ms56xx.c +++ b/src/main/drivers/barometer/barometer_ms56xx.c @@ -52,6 +52,9 @@ STATIC_UNIT_TESTED uint32_t ms56xx_up; // static result of pressure measurement STATIC_UNIT_TESTED uint16_t ms56xx_c[PROM_NB]; // on-chip ROM static uint8_t ms56xx_osr = CMD_ADC_4096; +// Target of the non-blocking ADC read, filled by the bus driver in the background +static uint8_t ms56xx_adc_buf[3]; + STATIC_UNIT_TESTED int8_t ms56xx_crc(uint16_t *prom) { int32_t i, j; @@ -82,32 +85,50 @@ STATIC_UNIT_TESTED int8_t ms56xx_crc(uint16_t *prom) return -1; } -static uint32_t ms56xx_read_adc(baroDev_t *baro) +static bool ms56xx_read_adc(baroDev_t *baro) +{ + // kick off the ADC read, ms56xx_adc_value() picks the result up once the bus is idle + return busReadBufStart(baro->busDev, CMD_ADC_READ, ms56xx_adc_buf, sizeof(ms56xx_adc_buf)); +} + +static uint32_t ms56xx_adc_value(void) { - uint8_t rxbuf[3]; - busReadBuf(baro->busDev, CMD_ADC_READ, rxbuf, 3); - return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2]; + return (ms56xx_adc_buf[0] << 16) | (ms56xx_adc_buf[1] << 8) | ms56xx_adc_buf[2]; } static bool ms56xx_start_ut(baroDev_t *baro) { - return busWrite(baro->busDev, CMD_ADC_CONV + CMD_ADC_D2 + ms56xx_osr, 1); + return busWriteStart(baro->busDev, CMD_ADC_CONV + CMD_ADC_D2 + ms56xx_osr, 1); } static bool ms56xx_get_ut(baroDev_t *baro) { - ms56xx_ut = ms56xx_read_adc(baro); + UNUSED(baro); + + const uint32_t value = ms56xx_adc_value(); + if (value == 0) { + return false; // ADC read before the conversion finished, redo the measurement + } + + ms56xx_ut = value; return true; } static bool ms56xx_start_up(baroDev_t *baro) { - return busWrite(baro->busDev, CMD_ADC_CONV + CMD_ADC_D1 + ms56xx_osr, 1); + return busWriteStart(baro->busDev, CMD_ADC_CONV + CMD_ADC_D1 + ms56xx_osr, 1); } static bool ms56xx_get_up(baroDev_t *baro) { - ms56xx_up = ms56xx_read_adc(baro); + UNUSED(baro); + + const uint32_t value = ms56xx_adc_value(); + if (value == 0) { + return false; // ADC read before the conversion finished, redo the measurement + } + + ms56xx_up = value; return true; } @@ -222,8 +243,10 @@ static bool deviceInit(baroDev_t *baro) baro->ut_delay = 10000; baro->up_delay = 10000; baro->start_ut = ms56xx_start_ut; + baro->read_ut = ms56xx_read_adc; baro->get_ut = ms56xx_get_ut; baro->start_up = ms56xx_start_up; + baro->read_up = ms56xx_read_adc; baro->get_up = ms56xx_get_up; return true; diff --git a/src/main/drivers/barometer/barometer_spl06.c b/src/main/drivers/barometer/barometer_spl06.c deleted file mode 100644 index 99870dcaee6..00000000000 --- a/src/main/drivers/barometer/barometer_spl06.c +++ /dev/null @@ -1,255 +0,0 @@ -/* - * This file is part of INAV. - * - * Cleanflight 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. - * - * Cleanflight 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 Cleanflight. If not, see . - */ - -#include -#include -#include - -#include -#include "build/build_config.h" -#include "common/utils.h" - -#include "drivers/time.h" -#include "drivers/io.h" -#include "drivers/bus.h" -#include "drivers/barometer/barometer.h" -#include "drivers/barometer/barometer_spl06.h" - -#if defined(USE_BARO_SPL06) - -// SPL06, address 0x76 - -typedef struct { - int16_t c0; - int16_t c1; - int32_t c00; - int32_t c10; - int16_t c01; - int16_t c11; - int16_t c20; - int16_t c21; - int16_t c30; -} spl06_coeffs_t; - - -spl06_coeffs_t spl06_cal; -// uncompensated pressure and temperature -static int32_t spl06_pressure_raw = 0; -static int32_t spl06_temperature_raw = 0; - -static int8_t spl06_samples_to_cfg_reg_value(uint8_t sample_rate) -{ - switch(sample_rate) - { - case 1: return 0; - case 2: return 1; - case 4: return 2; - case 8: return 3; - case 16: return 4; - case 32: return 5; - case 64: return 6; - case 128: return 7; - default: return -1; // invalid - } -} - -static int32_t spl06_raw_value_scale_factor(uint8_t oversampling_rate) -{ - switch(oversampling_rate) - { - case 1: return 524288; - case 2: return 1572864; - case 4: return 3670016; - case 8: return 7864320; - case 16: return 253952; - case 32: return 516096; - case 64: return 1040384; - case 128: return 2088960; - default: return -1; // invalid - } -} - -static bool spl06_start_temperature_measurement(baroDev_t * baro) -{ - return busWrite(baro->busDev, SPL06_MODE_AND_STATUS_REG, SPL06_MEAS_TEMPERATURE); -} - -static bool spl06_read_temperature(baroDev_t * baro) -{ - uint8_t data[SPL06_TEMPERATURE_LEN]; - int32_t spl06_temperature; - - bool ack = busReadBuf(baro->busDev, SPL06_TEMPERATURE_START_REG, data, SPL06_TEMPERATURE_LEN); - - if (ack) { - spl06_temperature = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2])); - spl06_temperature_raw = spl06_temperature; - } - - return ack; -} - -static bool spl06_start_pressure_measurement(baroDev_t * baro) -{ - return busWrite(baro->busDev, SPL06_MODE_AND_STATUS_REG, SPL06_MEAS_PRESSURE); -} - -static bool spl06_read_pressure(baroDev_t * baro) -{ - uint8_t data[SPL06_PRESSURE_LEN]; - int32_t spl06_pressure; - - bool ack = busReadBuf(baro->busDev, SPL06_PRESSURE_START_REG, data, SPL06_PRESSURE_LEN); - - if (ack) { - spl06_pressure = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2])); - spl06_pressure_raw = spl06_pressure; - } - - return ack; -} - -// Returns temperature in degrees centigrade -static float spl06_compensate_temperature(int32_t temperature_raw) -{ - const float t_raw_sc = (float)temperature_raw / spl06_raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING); - const float temp_comp = (float)spl06_cal.c0 / 2 + t_raw_sc * spl06_cal.c1; - return temp_comp; -} - -// Returns pressure in Pascal -static float spl06_compensate_pressure(int32_t pressure_raw, int32_t temperature_raw) -{ - const float p_raw_sc = (float)pressure_raw / spl06_raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING); - const float t_raw_sc = (float)temperature_raw / spl06_raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING); - - const float pressure_cal = (float)spl06_cal.c00 + p_raw_sc * ((float)spl06_cal.c10 + p_raw_sc * ((float)spl06_cal.c20 + p_raw_sc * spl06_cal.c30)); - const float p_temp_comp = t_raw_sc * ((float)spl06_cal.c01 + p_raw_sc * ((float)spl06_cal.c11 + p_raw_sc * spl06_cal.c21)); - - return pressure_cal + p_temp_comp; -} - -bool spl06_calculate(baroDev_t * baro, int32_t * pressure, int32_t * temperature) -{ - UNUSED(baro); - - if (pressure) { - *pressure = lrintf(spl06_compensate_pressure(spl06_pressure_raw, spl06_temperature_raw)); - } - - if (temperature) { - *temperature = lrintf(spl06_compensate_temperature(spl06_temperature_raw) * 100); - } - - return true; -} - -#define DETECTION_MAX_RETRY_COUNT 5 -static bool deviceDetect(busDevice_t * busDev) -{ - uint8_t chipId; - for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) { - delay(100); - bool ack = busRead(busDev, SPL06_CHIP_ID_REG, &chipId); - if (ack && chipId == SPL06_DEFAULT_CHIP_ID) { - return true; - } - }; - - return false; -} - -static bool read_calibration_coefficients(baroDev_t *baro) { - uint8_t sstatus; - if (!(busRead(baro->busDev, SPL06_MODE_AND_STATUS_REG, &sstatus) && (sstatus & SPL06_MEAS_CFG_COEFFS_RDY))) - return false; // error reading status or coefficients not ready - - uint8_t caldata[SPL06_CALIB_COEFFS_LEN]; - - if (!busReadBuf(baro->busDev, SPL06_CALIB_COEFFS_START, (uint8_t *)&caldata, SPL06_CALIB_COEFFS_LEN)) { - return false; - } - - spl06_cal.c0 = (caldata[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)caldata[0] << 4) | (((uint16_t)caldata[1] & 0xF0) >> 4); - spl06_cal.c1 = ((caldata[1] & 0x8 ? 0xF000 : 0) | ((uint16_t)caldata[1] & 0x0F) << 8) | (uint16_t)caldata[2]; - spl06_cal.c00 = (caldata[3] & 0x80 ? 0xFFF00000 : 0) | ((uint32_t)caldata[3] << 12) | ((uint32_t)caldata[4] << 4) | (((uint32_t)caldata[5] & 0xF0) >> 4); - spl06_cal.c10 = (caldata[5] & 0x8 ? 0xFFF00000 : 0) | (((uint32_t)caldata[5] & 0x0F) << 16) | ((uint32_t)caldata[6] << 8) | (uint32_t)caldata[7]; - spl06_cal.c01 = ((uint16_t)caldata[8] << 8) | ((uint16_t)caldata[9]); - spl06_cal.c11 = ((uint16_t)caldata[10] << 8) | (uint16_t)caldata[11]; - spl06_cal.c20 = ((uint16_t)caldata[12] << 8) | (uint16_t)caldata[13]; - spl06_cal.c21 = ((uint16_t)caldata[14] << 8) | (uint16_t)caldata[15]; - spl06_cal.c30 = ((uint16_t)caldata[16] << 8) | (uint16_t)caldata[17]; - - return true; -} - -static bool spl06_configure_measurements(baroDev_t *baro) -{ - uint8_t reg_value; - - reg_value = SPL06_TEMP_USE_EXT_SENSOR | spl06_samples_to_cfg_reg_value(SPL06_TEMPERATURE_OVERSAMPLING); - if (!busWrite(baro->busDev, SPL06_TEMPERATURE_CFG_REG, reg_value)) { - return false; - } - - reg_value = spl06_samples_to_cfg_reg_value(SPL06_PRESSURE_OVERSAMPLING); - if (!busWrite(baro->busDev, SPL06_PRESSURE_CFG_REG, reg_value)) { - return false; - } - - reg_value = 0; - if (SPL06_TEMPERATURE_OVERSAMPLING > 8) { - reg_value |= SPL06_TEMPERATURE_RESULT_BIT_SHIFT; - } - if (SPL06_PRESSURE_OVERSAMPLING > 8) { - reg_value |= SPL06_PRESSURE_RESULT_BIT_SHIFT; - } - if (!busWrite(baro->busDev, SPL06_INT_AND_FIFO_CFG_REG, reg_value)) { - return false; - } - - return true; -} - -bool spl06Detect(baroDev_t *baro) -{ - baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_SPL06, 0, OWNER_BARO); - if (baro->busDev == NULL) { - return false; - } - - busSetSpeed(baro->busDev, BUS_SPEED_STANDARD); - - if (!(deviceDetect(baro->busDev) && read_calibration_coefficients(baro) && spl06_configure_measurements(baro))) { - busDeviceDeInit(baro->busDev); - return false; - } - - baro->ut_delay = SPL06_MEASUREMENT_TIME(SPL06_TEMPERATURE_OVERSAMPLING) * 1000; - baro->get_ut = spl06_read_temperature; - baro->start_ut = spl06_start_temperature_measurement; - - baro->up_delay = SPL06_MEASUREMENT_TIME(SPL06_PRESSURE_OVERSAMPLING) * 1000; - baro->start_up = spl06_start_pressure_measurement; - baro->get_up = spl06_read_pressure; - - baro->calculate = spl06_calculate; - - return true; -} - -#endif diff --git a/src/main/drivers/barometer/barometer_spl06.h b/src/main/drivers/barometer/barometer_spl06.h deleted file mode 100644 index a92f1c57ece..00000000000 --- a/src/main/drivers/barometer/barometer_spl06.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * This file is part of INAV. - * - * Cleanflight 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. - * - * Cleanflight 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 Cleanflight. If not, see . - */ - -#pragma once - -#if !defined(SPL06_I2C_ADDR) -#define SPL06_I2C_ADDR 0x76 -#endif -#define SPL06_DEFAULT_CHIP_ID 0x10 - -#define SPL06_PRESSURE_START_REG 0x00 -#define SPL06_PRESSURE_LEN 3 // 24 bits, 3 bytes -#define SPL06_PRESSURE_B2_REG 0x00 // Pressure MSB Register -#define SPL06_PRESSURE_B1_REG 0x01 // Pressure middle byte Register -#define SPL06_PRESSURE_B0_REG 0x02 // Pressure LSB Register -#define SPL06_TEMPERATURE_START_REG 0x03 -#define SPL06_TEMPERATURE_LEN 3 // 24 bits, 3 bytes -#define SPL06_TEMPERATURE_B2_REG 0x03 // Temperature MSB Register -#define SPL06_TEMPERATURE_B1_REG 0x04 // Temperature middle byte Register -#define SPL06_TEMPERATURE_B0_REG 0x05 // Temperature LSB Register -#define SPL06_PRESSURE_CFG_REG 0x06 // Pressure config -#define SPL06_TEMPERATURE_CFG_REG 0x07 // Temperature config -#define SPL06_MODE_AND_STATUS_REG 0x08 // Mode and status -#define SPL06_INT_AND_FIFO_CFG_REG 0x09 // Interrupt and FIFO config -#define SPL06_INT_STATUS_REG 0x0A // Interrupt and FIFO config -#define SPL06_FIFO_STATUS_REG 0x0B // Interrupt and FIFO config -#define SPL06_RST_REG 0x0C // Softreset Register -#define SPL06_CHIP_ID_REG 0x0D // Chip ID Register -#define SPL06_CALIB_COEFFS_START 0x10 -#define SPL06_CALIB_COEFFS_END 0x21 - -#define SPL06_CALIB_COEFFS_LEN (SPL06_CALIB_COEFFS_END - SPL06_CALIB_COEFFS_START + 1) - -// TEMPERATURE_CFG_REG -#define SPL06_TEMP_USE_EXT_SENSOR (1<<7) - -// MODE_AND_STATUS_REG -#define SPL06_MEAS_PRESSURE (1<<0) // measure pressure -#define SPL06_MEAS_TEMPERATURE (1<<1) // measure temperature - -#define SPL06_MEAS_CFG_CONTINUOUS (1<<2) -#define SPL06_MEAS_CFG_PRESSURE_RDY (1<<4) -#define SPL06_MEAS_CFG_TEMPERATURE_RDY (1<<5) -#define SPL06_MEAS_CFG_SENSOR_RDY (1<<6) -#define SPL06_MEAS_CFG_COEFFS_RDY (1<<7) - -// INT_AND_FIFO_CFG_REG -#define SPL06_PRESSURE_RESULT_BIT_SHIFT (1<<2) // necessary for pressure oversampling > 8 -#define SPL06_TEMPERATURE_RESULT_BIT_SHIFT (1<<3) // necessary for temperature oversampling > 8 - -#define SPL06_PRESSURE_OVERSAMPLING 8 // oversampling 8 -#define SPL06_TEMPERATURE_OVERSAMPLING 8 // oversampling 8 - -#define SPL06_MEASUREMENT_TIME(oversampling) ((2 + lrintf(oversampling * 1.6)) + 1) // ms - -bool spl06Detect(baroDev_t *baro); - diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index 71410fb2748..3453d405e6d 100644 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -418,8 +418,95 @@ void busDeselectDevice(const busDevice_t * dev) #endif } -bool busIsBusy(const busDevice_t * dev) +bool busReadBufStart(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length) { +#if !defined(USE_SPI) && !defined(USE_I2C) + UNUSED(reg); + UNUSED(data); + UNUSED(length); +#endif + switch (dev->busType) { + case BUSTYPE_SPI: +#ifdef USE_SPI + // SPI is fast and synchronous, complete the read right away + return busReadBuf(dev, reg, data, length); +#else + return false; +#endif + + case BUSTYPE_I2C: +#ifdef USE_I2C + return i2cBusReadBufferStart(dev, reg, data, length); +#else + return false; +#endif + + default: + return false; + } +} + +bool busWriteStart(const busDevice_t * dev, uint8_t reg, uint8_t data) +{ +#if !defined(USE_SPI) && !defined(USE_I2C) + UNUSED(reg); + UNUSED(data); +#endif + switch (dev->busType) { + case BUSTYPE_SPI: +#ifdef USE_SPI + // SPI is fast and synchronous, complete the write right away + return busWrite(dev, reg, data); +#else + return false; +#endif + + case BUSTYPE_I2C: +#ifdef USE_I2C + return i2cBusWriteRegisterStart(dev, reg, data); +#else + return false; +#endif + + default: + return false; + } +} + +bool busWriteBufStart(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length) +{ +#if !defined(USE_SPI) && !defined(USE_I2C) + UNUSED(reg); + UNUSED(data); + UNUSED(length); +#endif + switch (dev->busType) { + case BUSTYPE_SPI: +#ifdef USE_SPI + // SPI is fast and synchronous, complete the write right away + return busWriteBuf(dev, reg, data, length); +#else + return false; +#endif + + case BUSTYPE_I2C: +#ifdef USE_I2C + return i2cBusWriteBufferStart(dev, reg, data, length); +#else + return false; +#endif + + default: + return false; + } +} + +bool busIsBusy(const busDevice_t * dev, bool * error) +{ + if (error) { + *error = false; + } + switch (dev->busType) { case BUSTYPE_SPI: #ifdef USE_SPI @@ -430,7 +517,7 @@ bool busIsBusy(const busDevice_t * dev) #endif case BUSTYPE_I2C: #ifdef USE_I2C - return i2cBusBusy(dev,NULL); + return i2cBusBusy(dev, error); #else UNUSED(dev); return false; diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 0416f516ea1..fe2a9fd2e00 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -284,6 +284,17 @@ extern const busDeviceDescriptor_t __busdev_registry_end[]; #endif // busTransfer and busTransferMultiple are supported only on full-duplex SPI bus +/* + * Result of one step of a sensor read made of several non-blocking transfers (status poll, trigger, data). + * The sensor layer calls the driver's readStart(dev, firstStep) with firstStep set for the first transfer and + * again after each transfer completed, until the last one is started; read() then only parses the data. + */ +typedef enum { + BUS_READ_STEP_BUSY = 0, // nothing was started, the bus is busy - call again later + BUS_READ_STEP_NEXT, // an intermediate transfer was started, call readStart() again once it is done + BUS_READ_STEP_LAST, // the final transfer was started, call read() once it is done +} busReadStepResult_e; + typedef struct busTransferDescriptor_s { uint8_t * rxBuf; const uint8_t * txBuf; @@ -295,6 +306,9 @@ bool i2cBusWriteBuffer(const busDevice_t * dev, uint8_t reg, const uint8_t * dat bool i2cBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data); bool i2cBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length); bool i2cBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data); +bool i2cBusReadBufferStart(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length); +bool i2cBusWriteRegisterStart(const busDevice_t * dev, uint8_t reg, uint8_t data); +bool i2cBusWriteBufferStart(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length); bool i2cBusBusy(const busDevice_t *dev, bool *error); bool spiBusInitHost(const busDevice_t * dev); @@ -336,4 +350,9 @@ bool busWrite(const busDevice_t * busdev, uint8_t reg, uint8_t data); bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length); bool busTransferMultiple(const busDevice_t * dev, busTransferDescriptor_t * buffers, int count); -bool busIsBusy(const busDevice_t * dev); +/* Non-blocking register access. Start the transfer, then poll busIsBusy() until it reports idle and check error. + * On SPI the transfer completes inside the start call. */ +bool busReadBufStart(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length); +bool busWriteStart(const busDevice_t * dev, uint8_t reg, uint8_t data); +bool busWriteBufStart(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length); // data must stay valid until busIsBusy() reports idle +bool busIsBusy(const busDevice_t * dev, bool * error); diff --git a/src/main/drivers/bus_busdev_i2c.c b/src/main/drivers/bus_busdev_i2c.c index fb255278d5b..370d7f308dc 100644 --- a/src/main/drivers/bus_busdev_i2c.c +++ b/src/main/drivers/bus_busdev_i2c.c @@ -53,13 +53,26 @@ bool i2cBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data) const bool allowRawAccess = (dev->flags & DEVFLAGS_USE_RAW_REGISTERS); return i2cRead(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, 1, data, allowRawAccess); } +bool i2cBusReadBufferStart(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length) +{ + const bool allowRawAccess = (dev->flags & DEVFLAGS_USE_RAW_REGISTERS); + return i2cReadStart(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, length, data, allowRawAccess); +} + +bool i2cBusWriteRegisterStart(const busDevice_t * dev, uint8_t reg, uint8_t data) +{ + const bool allowRawAccess = (dev->flags & DEVFLAGS_USE_RAW_REGISTERS); + return i2cWriteStart(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, data, allowRawAccess); +} + +bool i2cBusWriteBufferStart(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length) +{ + const bool allowRawAccess = (dev->flags & DEVFLAGS_USE_RAW_REGISTERS); + return i2cWriteBufferStart(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, length, data, allowRawAccess); +} + bool i2cBusBusy(const busDevice_t *dev, bool *error) -{ -#if defined(AT32F43x) - return i2cBusy(dev->busdev.i2c.i2cBus, error); -#endif - UNUSED(dev); - UNUSED(error); - return false; +{ + return i2cBusy(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, error); } #endif diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index bf685b1bfae..cd6f6578ebe 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -56,7 +56,7 @@ typedef struct i2cDevice_s { ioTag_t sda; rccPeriphTag_t rcc; I2CSpeed speed; -#if defined(STM32F7) || defined(STM32H7) || defined(AT32F43x) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(AT32F43x) uint8_t ev_irq; uint8_t er_irq; uint8_t af; @@ -68,6 +68,34 @@ void i2cInit(I2CDevice device); bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess); bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data, bool allowRawAccess); bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf, bool allowRawAccess); -bool i2cBusy(I2CDevice device, bool *error); + +// Non-blocking transfers: start returns immediately (false if the bus is busy or the transfer could not be started), +// completion and the result are polled with i2cBusy(). Platforms without an asynchronous driver complete the +// transfer synchronously inside the start call and report the bus as idle. +bool i2cReadStart(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf, bool allowRawAccess); +bool i2cWriteStart(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data, bool allowRawAccess); +bool i2cWriteBufferStart(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, const uint8_t *data, bool allowRawAccess); // data must stay valid until i2cBusy() reports idle +bool i2cBusy(I2CDevice device, uint8_t addr_, bool *error); // pending state and outcome of the given slave's transfer + +// Outcome of the last completed non-blocking transfer of every 7-bit slave address on one bus. The platform drivers +// keep one per bus so that a device still reads its own result after another device has used the bus in between. +typedef struct { + volatile uint32_t failed[4]; // one bit per address, set = the last transfer to that address failed +} i2cAddrResults_t; + +static inline void i2cAddrResultSet(i2cAddrResults_t *results, uint8_t addr_, bool error) +{ + const uint32_t mask = 1u << (addr_ & 31); + if (error) { + results->failed[(addr_ >> 5) & 3] |= mask; + } else { + results->failed[(addr_ >> 5) & 3] &= ~mask; + } +} + +static inline bool i2cAddrResultFailed(const i2cAddrResults_t *results, uint8_t addr_) +{ + return (results->failed[(addr_ >> 5) & 3] >> (addr_ & 31)) & 1u; +} uint16_t i2cGetErrorCounter(void); diff --git a/src/main/drivers/bus_i2c_at32f43x.c b/src/main/drivers/bus_i2c_at32f43x.c index 39e2d008ff6..6461717902b 100644 --- a/src/main/drivers/bus_i2c_at32f43x.c +++ b/src/main/drivers/bus_i2c_at32f43x.c @@ -19,6 +19,7 @@ #include #include +#include "build/debug.h" #include "io_impl.h" #include "drivers/io.h" #include "drivers/time.h" @@ -88,10 +89,49 @@ static volatile uint16_t i2cErrorCount = 0; typedef struct { bool initialised; i2c_handle_type handle; + + // Interrupt driven transfer started by i2cReadStart() / i2cWriteStart(). The i2c_application layer finishes it + // from the interrupt handlers: handle.status becomes I2C_END, or handle.error_code reports the failure. + bool pending; + uint8_t addr; // 7-bit address of the current / last non-blocking transfer + timeUs_t startUs; // start time, used to detect a transfer that never completes + uint8_t txByte; // data of a non-blocking single byte write, must outlive the call + i2cAddrResults_t addrResults; // outcome per slave address, survives i2cInit() } i2cState_t; static i2cState_t i2cState[I2CDEV_COUNT]; +// Transfer statistics shown with debug_mode = I2C, shared by all buses +typedef struct { + uint16_t eventIrqs; // interrupts during the current / last transfer + uint16_t errorIrqs; // error interrupts / aborted transfers since boot + uint16_t lastTransferUs; // start to completion of the last non-blocking transfer + uint16_t startCallUs; // time spent in the last i2cStartTransfer() call +} i2cDebugStats_t; + +static i2cDebugStats_t i2cStats; + +static void i2cDebugPublish(void) +{ + DEBUG_SET(DEBUG_I2C, 0, i2cStats.eventIrqs); + DEBUG_SET(DEBUG_I2C, 1, i2cStats.errorIrqs); + DEBUG_SET(DEBUG_I2C, 2, i2cStats.lastTransferUs); + DEBUG_SET(DEBUG_I2C, 5, i2cErrorCount); + DEBUG_SET(DEBUG_I2C, 7, i2cStats.startCallUs); +} + +static void i2cEventIrq(I2CDevice device) +{ + i2cStats.eventIrqs++; + i2c_evt_irq_handler(&i2cState[device].handle); +} + +static void i2cErrorIrq(I2CDevice device) +{ + i2cStats.errorIrqs++; + i2c_err_irq_handler(&i2cState[device].handle); +} + void i2cSetSpeed(uint8_t speed) { for (unsigned int i = 0; i < ARRAYLEN(i2cHardwareMap); i++) { @@ -102,42 +142,198 @@ void i2cSetSpeed(uint8_t speed) //I2C1_ERR_IRQHandler void I2C1_ERR_IRQHandler(void) { - i2c_err_irq_handler(&i2cState[I2CDEV_1].handle); + i2cErrorIrq(I2CDEV_1); } void I2C1_EVT_IRQHandler(void) { - i2c_evt_irq_handler(&i2cState[I2CDEV_1].handle); + i2cEventIrq(I2CDEV_1); } void I2C2_ERR_IRQHandler(void) { - i2c_err_irq_handler(&i2cState[I2CDEV_2].handle); + i2cErrorIrq(I2CDEV_2); } void I2C2_EVT_IRQHandler(void) { - i2c_evt_irq_handler(&i2cState[I2CDEV_2].handle); + i2cEventIrq(I2CDEV_2); } void I2C3_ERR_IRQHandler(void) { - i2c_err_irq_handler(&i2cState[I2CDEV_3].handle); + i2cErrorIrq(I2CDEV_3); } void I2C3_EVT_IRQHandler(void) { - i2c_evt_irq_handler(&i2cState[I2CDEV_3].handle); + i2cEventIrq(I2CDEV_3); } static bool i2cHandleHardwareFailure(I2CDevice device) { - (void)device; i2cErrorCount++; i2cInit(device); return false; } +// Pick up the outcome of a pending interrupt driven transfer and reset a stuck one +static void i2cPollPending(I2CDevice device) +{ + i2cState_t * state = &(i2cState[device]); + + if (!state->pending) { + return; + } + + // The end-of-transfer status is private to i2c_application.c, i2c_wait_end() with a zero timeout only probes it: + // I2C_ERR_TIMEOUT means the transfer is still running, anything else means it finished + if (state->handle.error_code != I2C_OK || i2c_wait_end(&state->handle, 0) != I2C_ERR_TIMEOUT) { + i2cStats.lastTransferUs = micros() - state->startUs; + if (state->handle.error_code != I2C_OK) { + i2cErrorCount++; // keep the "I2C Errors" count of status / MSP meaningful for non-blocking transfers + } + i2cAddrResultSet(&state->addrResults, state->addr, state->handle.error_code != I2C_OK); + state->pending = false; + } + else if (cmpTimeUs(micros(), state->startUs) >= I2C_TIMEOUT) { + // No completion within the timeout, the transfer is stuck - reset the peripheral + i2cHandleHardwareFailure(device); + state->handle.error_code = I2C_ERR_TIMEOUT; + i2cAddrResultSet(&state->addrResults, state->addr, true); + } +} + +// Block until no interrupt driven transfer is in progress +static void i2cWaitForIdle(I2CDevice device) +{ + do { + i2cPollPending(device); + } while (i2cState[device].pending); +} + +/* + * Start an interrupt driven transfer. The i2c_application layer sends the slave and register address synchronously + * (a few tens of microseconds), the data phase runs from the interrupt handlers. + * Returns false if the bus is busy or the transfer could not be started. + */ +static bool i2cStartTransfer(I2CDevice device, uint8_t addr_, uint8_t reg_, bool allowRawAccess, bool reading, uint8_t len, uint8_t *buf) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) + return false; + + i2cState_t * state = &(i2cState[device]); + + if (!state->initialised) + return false; + + if (reading && len == 0) + return false; + + i2cPollPending(device); + if (state->pending) + return false; + + const timeUs_t callStartUs = micros(); + i2cStats.eventIrqs = 0; + + i2c_status_type status; + + if (reading) { + if (reg_ == 0xFF && allowRawAccess) { + status = i2c_master_receive_int(&state->handle, addr_ << 1, buf, len, I2C_DEFAULT_TIMEOUT); + } + else { + status = i2c_memory_read_int(&state->handle, I2C_MEM_ADDR_WIDIH_8, addr_ << 1, reg_, buf, len, I2C_DEFAULT_TIMEOUT); + } + } + else if (len == 0) { + if (reg_ == 0xFF && allowRawAccess) { + return false; // nothing to send + } + // Register byte alone, sent as a plain one byte transfer + state->txByte = reg_; + status = i2c_master_transmit_int(&state->handle, addr_ << 1, &state->txByte, 1, I2C_DEFAULT_TIMEOUT); + } + else { + if (reg_ == 0xFF && allowRawAccess) { + status = i2c_master_transmit_int(&state->handle, addr_ << 1, buf, len, I2C_DEFAULT_TIMEOUT); + } + else { + status = i2c_memory_write_int(&state->handle, I2C_MEM_ADDR_WIDIH_8, addr_ << 1, reg_, buf, len, I2C_DEFAULT_TIMEOUT); + } + } + + if (status == I2C_ERR_STEP_1) { + return false; // bus still busy, try again later + } + + if (status != I2C_OK) { + /* wait for the stop flag to be set and clear it */ + i2c_wait_flag(&state->handle, I2C_STOPF_FLAG, I2C_EVENT_CHECK_NONE, I2C_DEFAULT_TIMEOUT); + i2c_flag_clear(state->handle.i2cx, I2C_STOPF_FLAG); + return i2cHandleHardwareFailure(device); + } + + state->addr = addr_; + state->startUs = callStartUs; + state->pending = true; + + i2cStats.startCallUs = micros() - callStartUs; + i2cDebugPublish(); + + return true; +} + +bool i2cReadStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf, bool allowRawAccess) +{ + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, true, len, buf); +} + +bool i2cWriteBufferStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) +{ + // the platform API takes a non-const buffer, the data is only read + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, false, len_, (uint8_t *)data); +} + +bool i2cWriteStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data, bool allowRawAccess) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) + return false; + + i2cPollPending(device); + if (i2cState[device].pending) + return false; // don't touch txByte while it may still be going out + + i2cState[device].txByte = data; + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, false, 1, &i2cState[device].txByte); +} + +bool i2cBusy(I2CDevice device, uint8_t addr_, bool *error) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + if (error) { + *error = true; + } + return false; + } + + i2cState_t * state = &(i2cState[device]); + + i2cPollPending(device); + + // Only the owner of the transfer on the bus is pending, everybody else gets the outcome of their own last transfer + const bool pending = state->pending && state->addr == addr_; + + if (error) { + *error = !pending && i2cAddrResultFailed(&state->addrResults, addr_); + } + + i2cDebugPublish(); + + return pending; +} + bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) { if (device == I2CINVALID) @@ -147,7 +343,10 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, if (!state->initialised) return false; - + + // Let a non-blocking transfer of another device finish first + i2cWaitForIdle(device); + i2c_status_type status; if ((reg_ == 0xFF || len_ == 0) && allowRawAccess) { @@ -200,7 +399,9 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t if (!state->initialised) return false; - //HAL_StatusTypeDef status; + // Let a non-blocking transfer of another device finish first + i2cWaitForIdle(device); + i2c_status_type status; if (reg_ == 0xFF && allowRawAccess) { status = i2c_master_receive(&state->handle, addr_ << 1,buf, len, I2C_DEFAULT_TIMEOUT); @@ -416,7 +617,8 @@ void i2cInit(I2CDevice device) nvic_irq_enable(hardware->ev_irq, NVIC_PRIO_I2C_EV,0); i2c_enable(pHandle->i2cx, TRUE); - + + state->pending = false; state->initialised = true; } @@ -463,27 +665,5 @@ static void i2cUnstick(IO_t scl, IO_t sda) IOHi(sda); // Set bus sda high } -bool i2cBusy(I2CDevice device, bool *error) -{ - if (device == I2CINVALID) - return true; - - i2cState_t * state = &(i2cState[device]); - - if (error) { - *error = state->handle.error_code==I2C_OK?false:true; - } - - if(state->handle.error_code ==I2C_OK){ - - if (i2c_flag_get(state->handle.i2cx, I2C_BUSYF_FLAG) == SET) - { - return true; - } - return false; - } - - return true; -} #endif diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index 3b1459ab1fb..523aa4ec141 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -20,6 +20,10 @@ #include +#include "build/debug.h" + +#include "common/utils.h" + #include "drivers/io.h" #include "drivers/time.h" @@ -101,10 +105,106 @@ static volatile uint16_t i2cErrorCount = 0; typedef struct { bool initialised; I2C_HandleTypeDef handle; + + // Interrupt driven transfer started by i2cReadStart() / i2cWriteStart(), finished by the HAL callbacks below + volatile bool busy; + volatile bool error; + uint8_t addr; // 7-bit address of the current / last non-blocking transfer + timeUs_t startUs; // start time, used to detect a transfer that never completes + uint8_t txByte; // data of a non-blocking single byte write, must outlive the call + i2cAddrResults_t addrResults; // outcome per slave address, survives i2cInit() } i2cState_t; static i2cState_t i2cState[I2CDEV_COUNT]; +// Transfer statistics shown with debug_mode = I2C, shared by all buses +typedef struct { + uint16_t eventIrqs; // event interrupts during the current / last transfer + uint16_t errorIrqs; // error interrupts since boot + uint16_t lastTransferUs; // start to completion of the last non-blocking transfer + uint16_t startCallUs; // time spent in the last i2cStartTransfer() call +} i2cDebugStats_t; + +static i2cDebugStats_t i2cStats; + +static void i2cDebugPublish(void) +{ + DEBUG_SET(DEBUG_I2C, 0, i2cStats.eventIrqs); + DEBUG_SET(DEBUG_I2C, 1, i2cStats.errorIrqs); + DEBUG_SET(DEBUG_I2C, 2, i2cStats.lastTransferUs); + DEBUG_SET(DEBUG_I2C, 5, i2cErrorCount); + DEBUG_SET(DEBUG_I2C, 7, i2cStats.startCallUs); +} + +static void i2cEventIrq(I2CDevice device) +{ + i2cStats.eventIrqs++; + HAL_I2C_EV_IRQHandler(&i2cState[device].handle); +} + +static void i2cErrorIrq(I2CDevice device) +{ + i2cStats.errorIrqs++; + HAL_I2C_ER_IRQHandler(&i2cState[device].handle); +} + +static i2cState_t *i2cStateFromHandle(const I2C_HandleTypeDef *hi2c) +{ + for (unsigned i = 0; i < ARRAYLEN(i2cState); i++) { + if (&i2cState[i].handle == hi2c) { + return &i2cState[i]; + } + } + return NULL; +} + +// HAL completion hooks, called from the I2C interrupt handlers once an interrupt driven transfer is over +static void i2cTransferFinished(I2C_HandleTypeDef *hi2c, bool error) +{ + i2cState_t *state = i2cStateFromHandle(hi2c); + if (!state) { + return; + } + + i2cStats.lastTransferUs = microsISR() - state->startUs; + state->error = error; + if (error) { + i2cErrorCount++; // keep the "I2C Errors" count of status / MSP meaningful for non-blocking transfers + } + i2cAddrResultSet(&state->addrResults, state->addr, error); + state->busy = false; +} + +void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c) +{ + i2cTransferFinished(hi2c, false); +} + +void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c) +{ + i2cTransferFinished(hi2c, false); +} + +void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *hi2c) +{ + i2cTransferFinished(hi2c, false); +} + +void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *hi2c) +{ + i2cTransferFinished(hi2c, false); +} + +void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) +{ + i2cTransferFinished(hi2c, true); +} + +void HAL_I2C_AbortCpltCallback(I2C_HandleTypeDef *hi2c) +{ + i2cTransferFinished(hi2c, true); +} + void i2cSetSpeed(uint8_t speed) { for (unsigned int i = 0; i < ARRAYLEN(i2cHardwareMap); i++) { @@ -114,43 +214,43 @@ void i2cSetSpeed(uint8_t speed) void I2C1_ER_IRQHandler(void) { - HAL_I2C_ER_IRQHandler(&i2cState[I2CDEV_1].handle); + i2cErrorIrq(I2CDEV_1); } void I2C1_EV_IRQHandler(void) { - HAL_I2C_EV_IRQHandler(&i2cState[I2CDEV_1].handle); + i2cEventIrq(I2CDEV_1); } void I2C2_ER_IRQHandler(void) { - HAL_I2C_ER_IRQHandler(&i2cState[I2CDEV_2].handle); + i2cErrorIrq(I2CDEV_2); } void I2C2_EV_IRQHandler(void) { - HAL_I2C_EV_IRQHandler(&i2cState[I2CDEV_2].handle); + i2cEventIrq(I2CDEV_2); } void I2C3_ER_IRQHandler(void) { - HAL_I2C_ER_IRQHandler(&i2cState[I2CDEV_3].handle); + i2cErrorIrq(I2CDEV_3); } void I2C3_EV_IRQHandler(void) { - HAL_I2C_EV_IRQHandler(&i2cState[I2CDEV_3].handle); + i2cEventIrq(I2CDEV_3); } #ifdef USE_I2C_DEVICE_4 void I2C4_ER_IRQHandler(void) { - HAL_I2C_ER_IRQHandler(&i2cState[I2CDEV_4].handle); + i2cErrorIrq(I2CDEV_4); } void I2C4_EV_IRQHandler(void) { - HAL_I2C_EV_IRQHandler(&i2cState[I2CDEV_4].handle); + i2cEventIrq(I2CDEV_4); } #endif @@ -162,6 +262,144 @@ static bool i2cHandleHardwareFailure(I2CDevice device) return false; } +// Reset the peripheral after a transfer that did not complete and record the failure for the transfer's owner +static void i2cRecoverStuckTransfer(I2CDevice device) +{ + i2cState_t * state = &(i2cState[device]); + + i2cHandleHardwareFailure(device); + state->error = true; + i2cAddrResultSet(&state->addrResults, state->addr, true); +} + +// Block until no interrupt driven transfer is in progress. Returns false if a stuck transfer had to be aborted. +static bool i2cWaitForIdle(I2CDevice device) +{ + i2cState_t * state = &(i2cState[device]); + const timeUs_t startUs = micros(); + + while (state->busy) { + if (cmpTimeUs(micros(), startUs) >= I2C_TIMEOUT) { + i2cRecoverStuckTransfer(device); + return false; + } + } + + return true; +} + +// Start an interrupt driven transfer. Returns false if the bus is busy or the transfer could not be started. +static bool i2cStartTransfer(I2CDevice device, uint8_t addr_, uint8_t reg_, bool allowRawAccess, bool reading, uint8_t len, uint8_t *buf) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) + return false; + + i2cState_t * state = &(i2cState[device]); + + if (!state->initialised || state->busy) + return false; + + if (reading && len == 0) + return false; + + const timeUs_t callStartUs = micros(); + i2cStats.eventIrqs = 0; + + state->error = false; + state->addr = addr_; + state->startUs = callStartUs; + state->busy = true; + + HAL_StatusTypeDef status; + + if (reading) { + if (reg_ == 0xFF && allowRawAccess) { + status = HAL_I2C_Master_Receive_IT(&state->handle, addr_ << 1, buf, len); + } + else { + status = HAL_I2C_Mem_Read_IT(&state->handle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT, buf, len); + } + } + else if (len == 0) { + if (reg_ == 0xFF && allowRawAccess) { + state->busy = false; + return false; // nothing to send + } + // Register byte alone, the HAL refuses zero length transfers + state->txByte = reg_; + status = HAL_I2C_Master_Transmit_IT(&state->handle, addr_ << 1, &state->txByte, 1); + } + else { + if (reg_ == 0xFF && allowRawAccess) { + status = HAL_I2C_Master_Transmit_IT(&state->handle, addr_ << 1, buf, len); + } + else { + status = HAL_I2C_Mem_Write_IT(&state->handle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT, buf, len); + } + } + + if (status != HAL_OK) { + state->busy = false; + if (status == HAL_BUSY) { + return false; // HAL is still finishing the previous transfer (STOP in progress), try again later + } + return i2cHandleHardwareFailure(device); + } + + i2cStats.startCallUs = micros() - callStartUs; + i2cDebugPublish(); + + return true; +} + +bool i2cReadStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf, bool allowRawAccess) +{ + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, true, len, buf); +} + +bool i2cWriteBufferStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) +{ + // the platform API takes a non-const buffer, the data is only read + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, false, len_, (uint8_t *)data); +} + +bool i2cWriteStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data, bool allowRawAccess) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT || i2cState[device].busy) + return false; // don't touch txByte while it may still be going out + + i2cState[device].txByte = data; + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, false, 1, &i2cState[device].txByte); +} + +bool i2cBusy(I2CDevice device, uint8_t addr_, bool *error) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + if (error) { + *error = true; + } + return false; + } + + i2cState_t * state = &(i2cState[device]); + + if (state->busy && cmpTimeUs(micros(), state->startUs) >= I2C_TIMEOUT) { + // No completion callback within the timeout, the transfer is stuck - reset the peripheral + i2cRecoverStuckTransfer(device); + } + + // Only the owner of the transfer on the bus is pending, everybody else gets the outcome of their own last transfer + const bool pending = state->busy && state->addr == addr_; + + if (error) { + *error = !pending && i2cAddrResultFailed(&state->addrResults, addr_); + } + + i2cDebugPublish(); + + return pending; +} + bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) { if (device == I2CINVALID) @@ -172,9 +410,18 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, if (!state->initialised) return false; + // Let a non-blocking transfer of another device finish first + if (!i2cWaitForIdle(device)) + return false; + HAL_StatusTypeDef status; - if ((reg_ == 0xFF || len_ == 0) && allowRawAccess) { + if (len_ == 0 && !(reg_ == 0xFF && allowRawAccess)) { + // Register byte alone, the HAL refuses zero length transfers + uint8_t regByte = reg_; + status = HAL_I2C_Master_Transmit(&state->handle, addr_ << 1, ®Byte, 1, I2C_DEFAULT_TIMEOUT); + } + else if ((reg_ == 0xFF || len_ == 0) && allowRawAccess) { status = HAL_I2C_Master_Transmit(&state->handle, addr_ << 1, (uint8_t *)data, len_, I2C_DEFAULT_TIMEOUT); } else { @@ -202,6 +449,10 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t if (!state->initialised) return false; + // Let a non-blocking transfer of another device finish first + if (!i2cWaitForIdle(device)) + return false; + HAL_StatusTypeDef status; if (reg_ == 0xFF && allowRawAccess) { @@ -333,6 +584,13 @@ void i2cInit(I2CDevice device) IOConfigGPIOAF(sda, IOCFG_I2C, hardware->af); // Init I2C peripheral + if (state->initialised) { + // Re-initialisation after a failure: drop whatever transfer the HAL still thinks is running + HAL_I2C_DeInit(pHandle); + } + state->busy = false; + state->error = false; + pHandle->Instance = hardware->dev; /* diff --git a/src/main/drivers/bus_i2c_rp2350.c b/src/main/drivers/bus_i2c_rp2350.c index cc1d5240c2d..16686aaf885 100644 --- a/src/main/drivers/bus_i2c_rp2350.c +++ b/src/main/drivers/bus_i2c_rp2350.c @@ -23,10 +23,12 @@ */ /* - * RP2350 I2C driver — blocking, Pico SDK hardware_i2c. + * RP2350 I2C driver — Pico SDK hardware_i2c. * - * Implements the INAV bus_i2c.h interface using the Pico SDK blocking API. - * No interrupts are used; all transfers complete before returning. + * Blocking transfers (i2cRead / i2cWrite) use the Pico SDK blocking API. + * Non-blocking transfers (i2cReadStart / i2cWriteStart) queue the commands into the controller FIFO and are + * completed from the I2C interrupt (STOP_DET / TX_ABRT), following the Betaflight RP2350 driver. Reads longer + * than the FIFO are refilled in batches on RX_FULL. A blocking call waits for a pending non-blocking one. * * Hardware mapping (Option C pin plan): * INAV I2CDEV_1 → RP2350 i2c1 → GP18 (SDA = PB2) / GP19 (SCL = PB3) @@ -47,12 +49,16 @@ #ifdef USE_I2C +#include "build/debug.h" + #include "drivers/bus_i2c.h" #include "drivers/io.h" #include "drivers/io_def.h" +#include "drivers/time.h" #include "hardware/gpio.h" #include "hardware/i2c.h" +#include "hardware/irq.h" #define RP2350_I2C_TIMEOUT_US 10000U /* 10 ms — generous for 100–800 kHz */ @@ -75,13 +81,155 @@ static const uint32_t speedToBaud[] = { static uint32_t i2cBaudrate = 400000U; static uint16_t i2cErrorCount = 0; +// Interrupts used by a non-blocking transfer +#define I2C_XFER_INTR (I2C_IC_INTR_STAT_R_STOP_DET_BITS | I2C_IC_INTR_STAT_R_TX_ABRT_BITS | \ + I2C_IC_INTR_STAT_R_TX_OVER_BITS | I2C_IC_INTR_STAT_R_RX_OVER_BITS) +#define I2C_FIFO_DEPTH 16U + +typedef enum { + I2C_XFER_IDLE = 0, + I2C_XFER_ACTIVE, // all commands are in the FIFO, waiting for STOP_DET + I2C_XFER_READ_BATCHES, // long read, more read commands go in as the RX FIFO fills +} i2cXferState_e; + typedef struct { i2c_inst_t *hw; bool initialised; + bool irqInstalled; + + // Non-blocking transfer context, shared with the interrupt handler + volatile i2cXferState_e xferState; + volatile bool error; // last non-blocking transfer failed + bool read; + uint8_t *data; + uint8_t len; + volatile uint8_t transferred; + uint8_t addr; // 7-bit address of the current / last non-blocking transfer + timeUs_t startUs; // start time, used to detect a transfer that never completes + uint8_t txByte; // data of a non-blocking single byte write, must outlive the call + i2cAddrResults_t addrResults; // outcome per slave address, survives i2cInit() } rp2350_i2c_state_t; static rp2350_i2c_state_t i2cState[I2CDEV_COUNT]; +// Which INAV bus sits on i2c0 / i2c1, for the interrupt handlers +static I2CDevice i2cDeviceForInstance[2] = { I2CINVALID, I2CINVALID }; + +// Transfer statistics shown with debug_mode = I2C, shared by all buses +typedef struct { + uint16_t eventIrqs; // interrupts during the current / last transfer + uint16_t errorIrqs; // error interrupts / aborted transfers since boot + uint16_t lastTransferUs; // start to completion of the last non-blocking transfer + uint16_t startCallUs; // time spent in the last i2cStartTransfer() call +} i2cDebugStats_t; + +static i2cDebugStats_t i2cStats; + +static void i2cDebugPublish(void) +{ + DEBUG_SET(DEBUG_I2C, 0, i2cStats.eventIrqs); + DEBUG_SET(DEBUG_I2C, 1, i2cStats.errorIrqs); + DEBUG_SET(DEBUG_I2C, 2, i2cStats.lastTransferUs); + DEBUG_SET(DEBUG_I2C, 5, i2cErrorCount); + DEBUG_SET(DEBUG_I2C, 7, i2cStats.startCallUs); +} + +static void i2cQueueReadCommands(i2c_hw_t *hw, uint8_t count, bool restartFirst, bool stopLast) +{ + for (uint8_t i = 0; i < count; i++) { + uint32_t cmd = I2C_IC_DATA_CMD_CMD_BITS; + if (i == 0 && restartFirst) { + cmd |= I2C_IC_DATA_CMD_RESTART_BITS; + } + if (i == count - 1 && stopLast) { + cmd |= I2C_IC_DATA_CMD_STOP_BITS; + } + hw->data_cmd = cmd; + } +} + +static void i2cDrainRxFifo(rp2350_i2c_state_t *state, i2c_hw_t *hw) +{ + while (hw->rxflr > 0 && state->transferred < state->len) { + state->data[state->transferred++] = (uint8_t)(hw->data_cmd & I2C_IC_DATA_CMD_DAT_BITS); + } +} + +static void i2cFinishTransfer(rp2350_i2c_state_t *state, i2c_hw_t *hw, bool error) +{ + hw->intr_mask = 0; + state->error = error; + if (error) { + i2cErrorCount++; + i2cStats.errorIrqs++; + } + i2cStats.lastTransferUs = micros() - state->startUs; + i2cAddrResultSet(&state->addrResults, state->addr, error); + state->xferState = I2C_XFER_IDLE; +} + +static void i2cIrqHandler(I2CDevice device) +{ + if (device == I2CINVALID) { + return; + } + + rp2350_i2c_state_t *state = &i2cState[device]; + i2c_hw_t *hw = i2c_get_hw(state->hw); + const uint32_t intrStat = hw->intr_stat; + + i2cStats.eventIrqs++; + + if (intrStat & I2C_IC_INTR_STAT_R_TX_ABRT_BITS) { + (void)hw->clr_tx_abrt; // NACK or arbitration loss, the controller flushed the FIFO + i2cFinishTransfer(state, hw, true); + return; + } + + if (intrStat & I2C_IC_INTR_STAT_R_TX_OVER_BITS) { + (void)hw->clr_tx_over; + i2cFinishTransfer(state, hw, true); + return; + } + + if (intrStat & I2C_IC_INTR_STAT_R_RX_OVER_BITS) { + (void)hw->clr_rx_over; + i2cFinishTransfer(state, hw, true); + return; + } + + if (intrStat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) { + (void)hw->clr_stop_det; + if (state->read) { + i2cDrainRxFifo(state, hw); + } + i2cFinishTransfer(state, hw, state->read && state->transferred < state->len); + return; + } + + if ((intrStat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) && state->xferState == I2C_XFER_READ_BATCHES) { + i2cDrainRxFifo(state, hw); + const uint8_t remaining = state->len - state->transferred; + if (remaining > 0) { + const bool finalBatch = remaining <= I2C_FIFO_DEPTH; + i2cQueueReadCommands(hw, finalBatch ? remaining : I2C_FIFO_DEPTH, false, finalBatch); + if (finalBatch) { + hw->intr_mask = I2C_XFER_INTR; // no more refills, just wait for the STOP + } + } + } +} + +static void i2c0IrqHandler(void) +{ + i2cIrqHandler(i2cDeviceForInstance[0]); +} + +static void i2c1IrqHandler(void) +{ + i2cIrqHandler(i2cDeviceForInstance[1]); +} + /* * Map a GPIO number to i2c0 or i2c1. * RP2350 I2C mux pattern: (gpio / 2) is odd → i2c1, even → i2c0. @@ -146,10 +294,143 @@ void i2cInit(I2CDevice device) i2c_init(hw, i2cBaudrate); + i2c_hw_t *regs = i2c_get_hw(hw); + regs->intr_mask = 0; // interrupts are enabled per non-blocking transfer + regs->rx_tl = I2C_FIFO_DEPTH - 2; // RX_FULL fires with 15 bytes queued, in time to refill a long read + + const uint instance = i2c_hw_index(hw); + i2cDeviceForInstance[instance] = device; + + if (!i2cState[device].irqInstalled) { + const uint irq = (instance == 0) ? I2C0_IRQ : I2C1_IRQ; + irq_set_exclusive_handler(irq, (instance == 0) ? i2c0IrqHandler : i2c1IrqHandler); + irq_set_enabled(irq, true); + i2cState[device].irqInstalled = true; + } + + i2cState[device].xferState = I2C_XFER_IDLE; + i2cState[device].error = false; i2cState[device].hw = hw; i2cState[device].initialised = true; } +// Reset a transfer that did not complete and record the failure for its owner. Returns true while a transfer is on the bus. +static bool i2cPollTransfer(I2CDevice device) +{ + rp2350_i2c_state_t *state = &i2cState[device]; + + if (state->xferState != I2C_XFER_IDLE && cmpTimeUs(micros(), state->startUs) >= I2C_TIMEOUT) { + // No STOP_DET within the timeout, the transfer is stuck - reset the controller + i2c_get_hw(state->hw)->intr_mask = 0; + state->xferState = I2C_XFER_IDLE; + i2cErrorCount++; + i2cInit(device); + state->error = true; + i2cAddrResultSet(&state->addrResults, state->addr, true); + } + + return state->xferState != I2C_XFER_IDLE; +} + +// Block until no non-blocking transfer is in progress +static void i2cWaitForIdle(I2CDevice device) +{ + while (i2cPollTransfer(device)) { + } +} + +/* + * Queue a transfer into the controller FIFO and let the interrupt finish it. + * Returns false if the bus is busy or the transfer does not fit. + */ +static bool i2cStartTransfer(I2CDevice device, uint8_t addr_, uint8_t reg_, bool allowRawAccess, bool reading, uint8_t len, uint8_t *buf) +{ + if (device < 0 || device >= I2CDEV_COUNT || !i2cState[device].initialised) { + return false; + } + + rp2350_i2c_state_t *state = &i2cState[device]; + i2c_hw_t *hw = i2c_get_hw(state->hw); + const bool useRegister = !(reg_ == 0xFF && allowRawAccess); + + if (reading && len == 0) { + return false; + } + + if (!reading && len + (useRegister ? 1U : 0U) > I2C_FIFO_DEPTH) { + return false; // writes are not refilled, they must fit the FIFO + } + + if (i2cPollTransfer(device) || (hw->status & I2C_IC_STATUS_ACTIVITY_BITS)) { + return false; // previous transfer still on the bus + } + + const timeUs_t callStartUs = micros(); + i2cStats.eventIrqs = 0; + + state->read = reading; + state->data = buf; + state->len = len; + state->transferred = 0; + state->error = false; + state->addr = addr_; + state->startUs = callStartUs; + + hw->enable = 0; + hw->tar = addr_; + hw->enable = 1; + + // Drop flags left behind by blocking SDK transfers, they would end this transfer prematurely + (void)hw->clr_intr; + (void)hw->clr_tx_abrt; + + if (useRegister) { + hw->data_cmd = (uint32_t)reg_ | ((!reading && len == 0) ? I2C_IC_DATA_CMD_STOP_BITS : 0); + } + + if (reading) { + // The register byte takes one FIFO slot, the first batch of read commands fills the rest + const uint8_t firstBatch = (len <= I2C_FIFO_DEPTH - 1) ? len : (I2C_FIFO_DEPTH - 1); + const bool singleBatch = (firstBatch == len); + state->xferState = singleBatch ? I2C_XFER_ACTIVE : I2C_XFER_READ_BATCHES; + i2cQueueReadCommands(hw, firstBatch, useRegister, singleBatch); + hw->intr_mask = I2C_XFER_INTR | (singleBatch ? 0 : I2C_IC_INTR_STAT_R_RX_FULL_BITS); + } + else { + state->xferState = I2C_XFER_ACTIVE; + for (uint8_t i = 0; i < len; i++) { + hw->data_cmd = (uint32_t)buf[i] | ((i == len - 1) ? I2C_IC_DATA_CMD_STOP_BITS : 0); + } + hw->intr_mask = I2C_XFER_INTR; + } + + i2cStats.startCallUs = micros() - callStartUs; + i2cDebugPublish(); + + return true; +} + +bool i2cReadStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf, bool allowRawAccess) +{ + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, true, len, buf); +} + +bool i2cWriteBufferStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) +{ + // the platform API takes a non-const buffer, the data is only read + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, false, len_, (uint8_t *)data); +} + +bool i2cWriteStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data, bool allowRawAccess) +{ + if (device < 0 || device >= I2CDEV_COUNT || i2cPollTransfer(device)) { + return false; // don't touch txByte while it may still be going out + } + + i2cState[device].txByte = data; + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, false, 1, &i2cState[device].txByte); +} + bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) { @@ -157,6 +438,9 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, return false; } + // Let a non-blocking transfer of another device finish first + i2cWaitForIdle(device); + i2c_inst_t *hw = i2cState[device].hw; int ret; @@ -193,6 +477,9 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, return false; } + // Let a non-blocking transfer of another device finish first + i2cWaitForIdle(device); + i2c_inst_t *hw = i2cState[device].hw; int ret; @@ -216,14 +503,27 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, return true; } -bool i2cBusy(I2CDevice device, bool *error) +bool i2cBusy(I2CDevice device, uint8_t addr_, bool *error) { - UNUSED(device); + if (device < 0 || device >= I2CDEV_COUNT) { + if (error) { + *error = true; + } + return false; + } + + rp2350_i2c_state_t *state = &i2cState[device]; + + // Only the owner of the transfer on the bus is pending, everybody else gets the outcome of their own last transfer + const bool pending = i2cPollTransfer(device) && state->addr == addr_; + if (error) { - *error = false; + *error = !pending && i2cAddrResultFailed(&state->addrResults, addr_); } - /* Blocking implementation — transfers complete before returning */ - return false; + + i2cDebugPublish(); + + return pending; } uint16_t i2cGetErrorCounter(void) diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index 5c7d866970e..aa6f13c30a7 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -290,6 +290,33 @@ bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t * return true; } +// Non-blocking transfers are not implemented for this platform yet - complete them synchronously +bool i2cReadStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf, bool allowRawAccess) +{ + return i2cRead(device, addr_, reg_, len, buf, allowRawAccess); +} + +bool i2cWriteBufferStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) +{ + return i2cWriteBuffer(device, addr_, reg_, len_, data, allowRawAccess); +} + +bool i2cWriteStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data, bool allowRawAccess) +{ + return i2cWrite(device, addr_, reg_, data, allowRawAccess); +} + +bool i2cBusy(I2CDevice device, uint8_t addr_, bool *error) +{ + UNUSED(device); + UNUSED(addr_); + if (error) { + *error = false; + } + // Blocking driver - a transfer is finished when the start call returns + return false; +} + uint16_t i2cGetErrorCounter(void) { return i2cErrorCount; diff --git a/src/main/drivers/bus_i2c_stm32f40x.c b/src/main/drivers/bus_i2c_stm32f40x.c index cc0ed558fc3..cd2cd393824 100644 --- a/src/main/drivers/bus_i2c_stm32f40x.c +++ b/src/main/drivers/bus_i2c_stm32f40x.c @@ -1,27 +1,37 @@ /* - * This file is part of Cleanflight. + * This file is part of INAV. * - * Cleanflight is free software: you can redistribute it and/or modify + * INAV 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. * - * Cleanflight is distributed in the hope that it will be useful, + * INAV 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 Cleanflight. If not, see . + * along with INAV. If not, see . + * + * Interrupt driven I2C master for STM32F4. The transfer engine (event and + * error handlers) is derived from the Betaflight bus_i2c_stm32f4xx.c driver. + * + * A transfer is started from task context and completed by the I2Cx_EV and + * I2Cx_ER interrupts. i2cRead()/i2cWrite() block until the transfer is done, + * i2cReadStart()/i2cWriteStart() return immediately and the caller polls + * i2cBusy(). A blocking call issued while a non-blocking transfer is in + * progress waits for it to finish first, so the two can share a bus. */ #include #include #include +#include #include -#include "build/atomic.h" +#include "build/debug.h" #include "common/utils.h" @@ -32,16 +42,15 @@ #include "drivers/nvic.h" #include "io_impl.h" #include "rcc.h" -#include "drivers/light_led.h" #ifndef SOFT_I2C -static void i2cUnstick(IO_t scl, IO_t sda); +// Upper bound for the short waits on START/STOP bit release inside the handlers. +// Normally these clear within one SCL period; a longer wait means a stuck bus. +#define I2C_BIT_RELEASE_TIMEOUT_US 1000 #define GPIO_AF_I2C GPIO_AF_I2C1 -#ifdef STM32F4 - #if defined(USE_I2C_PULLUP) #define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP) #else @@ -55,405 +64,145 @@ static void i2cUnstick(IO_t scl, IO_t sda); #define I2C1_SDA PB9 #endif -#else - -#ifndef I2C1_SCL -#define I2C1_SCL PB6 -#endif -#ifndef I2C1_SDA -#define I2C1_SDA PB7 -#endif -#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_50MHz) - -#endif - #ifndef I2C2_SCL #define I2C2_SCL PB10 #endif - #ifndef I2C2_SDA #define I2C2_SDA PB11 #endif -#ifdef STM32F4 #ifndef I2C3_SCL #define I2C3_SCL PA8 #endif #ifndef I2C3_SDA #define I2C3_SDA PB4 #endif -#endif - -typedef enum { - I2C_STATE_STOPPED = 0, - I2C_STATE_STOPPING, - I2C_STATE_STARTING, - I2C_STATE_STARTING_WAIT, - - I2C_STATE_R_ADDR, - I2C_STATE_R_ADDR_WAIT, - I2C_STATE_R_REGISTER, - I2C_STATE_R_REGISTER_WAIT, - I2C_STATE_R_RESTARTING, - I2C_STATE_R_RESTARTING_WAIT, - I2C_STATE_R_RESTART_ADDR, - I2C_STATE_R_RESTART_ADDR_WAIT, - I2C_STATE_R_TRANSFER_EQ1, - I2C_STATE_R_TRANSFER_EQ2, - I2C_STATE_R_TRANSFER_GE2, - - I2C_STATE_W_ADDR, - I2C_STATE_W_ADDR_WAIT, - I2C_STATE_W_REGISTER, - I2C_STATE_W_TRANSFER_WAIT, - I2C_STATE_W_TRANSFER, - - I2C_STATE_NACK, - I2C_STATE_BUS_ERROR, -} i2cState_t; - -typedef enum { - I2C_TXN_READ, - I2C_TXN_WRITE -} i2cTransferDirection_t; typedef struct i2cBusState_s { - I2CDevice device; bool initialized; - i2cState_t state; - timeUs_t timeout; - - /* Active transfer */ - bool allowRawAccess; - uint8_t addr; // device address - i2cTransferDirection_t rw; // direction - uint8_t reg; // register - uint32_t len; // buffer length - uint8_t *buf; // buffer - bool txnOk; + volatile bool busy; // transfer in progress + volatile bool error; // last transfer failed (NACK, bus error, arbitration lost, overrun or timeout) + timeUs_t startUs; // start of the current transfer, used to detect one that never completes + + // Active transfer + bool reading; + bool rawAccess; // no register address phase + uint8_t addr; // 7-bit address, pre-shifted + uint8_t reg; + int16_t bytes; + uint8_t * readBuf; + const uint8_t * writeBuf; + uint8_t txByte; // storage for the data of a non-blocking single byte write + + // Event handler state + bool subaddressSent; + int16_t index; // next byte index, -1 = register address goes out next } i2cBusState_t; static volatile uint16_t i2cErrorCount = 0; -static i2cDevice_t i2cHardwareMap[] = { - { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .speed = I2C_SPEED_400KHZ }, - { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .speed = I2C_SPEED_400KHZ }, -#ifdef STM32F4 - { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .speed = I2C_SPEED_400KHZ } -#endif -}; - -static i2cBusState_t busState[I2CDEV_COUNT] = { { 0 } }; - -static void i2cResetInterface(i2cBusState_t * i2cBusState) +// Transfer statistics shown with debug_mode = I2C, shared by all buses +typedef struct { + timeUs_t startUs; // when the current transfer was started + uint16_t eventIrqs; // event interrupts during the current / last transfer + uint16_t errorIrqs; // error interrupts since boot + uint16_t lastTransferUs; // start to completion of the last transfer + uint16_t maxIrqUs; // longest single event handler run + uint16_t maxBitWaitUs; // longest wait for a START / STOP bit release + uint16_t startCr2; // interrupt enables found in CR2 when a transfer was started, should be 0 + uint16_t startCallUs; // time spent in the last i2cStartTransfer() call +} i2cDebugStats_t; + +static i2cDebugStats_t i2cStats; + +static void i2cDebugPublish(void) { - const i2cDevice_t * i2c = &(i2cHardwareMap[i2cBusState->device]); - IO_t scl = IOGetByTag(i2c->scl); - IO_t sda = IOGetByTag(i2c->sda); - - i2cErrorCount++; - i2cUnstick(scl, sda); - i2cInit(i2cBusState->device); + DEBUG_SET(DEBUG_I2C, 0, i2cStats.eventIrqs); + DEBUG_SET(DEBUG_I2C, 1, i2cStats.errorIrqs); + DEBUG_SET(DEBUG_I2C, 2, i2cStats.lastTransferUs); + DEBUG_SET(DEBUG_I2C, 3, i2cStats.maxIrqUs); + DEBUG_SET(DEBUG_I2C, 4, i2cStats.maxBitWaitUs); + DEBUG_SET(DEBUG_I2C, 5, i2cErrorCount); + DEBUG_SET(DEBUG_I2C, 6, i2cStats.startCr2); + DEBUG_SET(DEBUG_I2C, 7, i2cStats.startCallUs); } -static void i2cStateMachine(i2cBusState_t * i2cBusState, const timeUs_t currentTicksUs) -{ - I2C_TypeDef * I2Cx = i2cHardwareMap[i2cBusState->device].dev; - - switch (i2cBusState->state) { - case I2C_STATE_BUS_ERROR: - i2cResetInterface(i2cBusState); - i2cBusState->state = I2C_STATE_STOPPED; - break; - - case I2C_STATE_STOPPING: - // Wait for stop bit to clear - // RM0090: When the STOP, START or PEC bit is set, the software must not perform any write access - // to I2C_CR1 before this bit is cleared by hardware. Otherwise there is a risk of setting a second STOP, START or PEC request. - if ((I2Cx->CR1 & I2C_CR1_STOP) == 0) { - i2cBusState->state = I2C_STATE_STOPPED; - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; - - case I2C_STATE_STOPPED: - // Stick here - break; - - case I2C_STATE_STARTING: - I2C_NACKPositionConfig(I2Cx, I2C_NACKPosition_Current); - I2C_AcknowledgeConfig(I2Cx, ENABLE); - I2C_GenerateSTART(I2Cx, ENABLE); - i2cBusState->state = I2C_STATE_STARTING_WAIT; - i2cBusState->timeout = currentTicksUs; - FALLTHROUGH; - - case I2C_STATE_STARTING_WAIT: - if (I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT) != ERROR) { - if (i2cBusState->rw == I2C_TXN_READ) { - // Special case - no register address - if (i2cBusState->reg == 0xFF && i2cBusState->allowRawAccess) { - i2cBusState->state = I2C_STATE_R_RESTART_ADDR; - } - else { - i2cBusState->state = I2C_STATE_R_ADDR; - } - } - else { - i2cBusState->state = I2C_STATE_W_ADDR; - } - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; - - case I2C_STATE_R_ADDR: - I2C_Send7bitAddress(I2Cx, i2cBusState->addr, I2C_Direction_Transmitter); - i2cBusState->state = I2C_STATE_R_ADDR_WAIT; - i2cBusState->timeout = currentTicksUs; - FALLTHROUGH; - - case I2C_STATE_R_ADDR_WAIT: - if (I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED) != ERROR) { - i2cBusState->state = I2C_STATE_R_REGISTER; - } - else if (I2C_GetFlagStatus(I2Cx, I2C_FLAG_AF) != RESET) { - i2cBusState->state = I2C_STATE_NACK; - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; - - case I2C_STATE_R_REGISTER: /* Send Register address */ - I2C_SendData(I2Cx, i2cBusState->reg); - i2cBusState->state = I2C_STATE_R_REGISTER_WAIT; - i2cBusState->timeout = currentTicksUs; - FALLTHROUGH; - - case I2C_STATE_R_REGISTER_WAIT: - if (I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED) != ERROR) { - if (i2cBusState->len == 0) { - I2C_GenerateSTOP(I2Cx, ENABLE); - i2cBusState->timeout = currentTicksUs; - i2cBusState->state = I2C_STATE_STOPPING; - } - else { - i2cBusState->state = I2C_STATE_R_RESTARTING; - } - } - else if (I2C_GetFlagStatus(I2Cx, I2C_FLAG_AF) != RESET) { - i2cBusState->state = I2C_STATE_NACK; - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; - - case I2C_STATE_R_RESTARTING: - I2C_GenerateSTART(I2Cx, ENABLE); - i2cBusState->state = I2C_STATE_R_RESTARTING_WAIT; - i2cBusState->timeout = currentTicksUs; - FALLTHROUGH; - - case I2C_STATE_R_RESTARTING_WAIT: - if (I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT) != ERROR) { - i2cBusState->state = I2C_STATE_R_RESTART_ADDR; - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; - - case I2C_STATE_R_RESTART_ADDR: - I2C_Send7bitAddress(I2Cx, i2cBusState->addr, I2C_Direction_Receiver); - i2cBusState->state = I2C_STATE_R_RESTART_ADDR_WAIT; - i2cBusState->timeout = currentTicksUs; - FALLTHROUGH; - - case I2C_STATE_R_RESTART_ADDR_WAIT: - if (I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED) != ERROR) { - if (i2cBusState->len == 1) { - // This TXN is 1-byte, disable ACK and generate stop early - I2C_AcknowledgeConfig(I2Cx, DISABLE); - - ATOMIC_BLOCK(NVIC_PRIO_MAX) { - (void) I2Cx->SR2; - I2C_GenerateSTOP(I2Cx, ENABLE); - } - - i2cBusState->state = I2C_STATE_R_TRANSFER_EQ1; - } - else if (i2cBusState->len == 2) { - // 2-byte transaction, disable ACK - I2C_NACKPositionConfig(I2Cx, I2C_NACKPosition_Next); - ATOMIC_BLOCK(NVIC_PRIO_MAX) { - (void) I2Cx->SR2; - I2C_AcknowledgeConfig(I2Cx, DISABLE); - } - - i2cBusState->state = I2C_STATE_R_TRANSFER_EQ2; - } - else { - (void) I2Cx->SR2; // Clear ADDR flag - i2cBusState->state = I2C_STATE_R_TRANSFER_GE2; - } - - i2cBusState->timeout = currentTicksUs; - } - else if (I2C_GetFlagStatus(I2Cx, I2C_FLAG_AF) != RESET) { - i2cBusState->state = I2C_STATE_NACK; - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; - - case I2C_STATE_R_TRANSFER_EQ1: - if (I2C_GetFlagStatus(I2Cx, I2C_FLAG_RXNE) != RESET) { - *i2cBusState->buf++ = I2C_ReceiveData(I2Cx); - i2cBusState->len--; +static i2cDevice_t i2cHardwareMap[] = { + { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn }, + { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn }, + { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn }, +}; - // This was the last successful byte - i2cBusState->txnOk = true; - i2cBusState->timeout = currentTicksUs; - i2cBusState->state = I2C_STATE_STOPPING; - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; +static i2cBusState_t busState[ARRAYLEN(i2cHardwareMap)]; - case I2C_STATE_R_TRANSFER_EQ2: - if (I2C_GetFlagStatus(I2Cx, I2C_FLAG_BTF) != RESET) { - ATOMIC_BLOCK(NVIC_PRIO_MAX) { - I2C_GenerateSTOP(I2Cx,ENABLE); - *i2cBusState->buf++ = I2C_ReceiveData(I2Cx); - } +// Per address outcome of the last transfer. Kept outside busState so that the reset in i2cInit() does not wipe the +// results of the other devices on the bus +static i2cAddrResults_t busAddrResults[ARRAYLEN(i2cHardwareMap)]; - *i2cBusState->buf++ = I2C_ReceiveData(I2Cx); - i2cBusState->len =- 2; +static void i2cUnstick(IO_t scl, IO_t sda); +static void i2cEventHandler(I2CDevice device); +static void i2cErrorHandler(I2CDevice device); - // This was the last successful byte - i2cBusState->txnOk = true; - i2cBusState->timeout = currentTicksUs; - i2cBusState->state = I2C_STATE_STOPPING; - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; +static bool i2cIsValidDevice(I2CDevice device) +{ + return device != I2CINVALID && (unsigned)device < ARRAYLEN(i2cHardwareMap); +} - case I2C_STATE_R_TRANSFER_GE2: - if (I2C_GetFlagStatus(I2Cx, I2C_FLAG_BTF) != RESET) { - if (i2cBusState->len == 3) { - I2C_AcknowledgeConfig(I2Cx, DISABLE); // clear ack bit +void I2C1_ER_IRQHandler(void) +{ + i2cErrorHandler(I2CDEV_1); +} - ATOMIC_BLOCK(NVIC_PRIO_MAX) { - *i2cBusState->buf++ = I2C_ReceiveData(I2Cx); - I2C_GenerateSTOP(I2Cx,ENABLE); - } +void I2C1_EV_IRQHandler(void) +{ + i2cEventHandler(I2CDEV_1); +} - *i2cBusState->buf++ = I2C_ReceiveData(I2Cx); - i2cBusState->len -= 2; +void I2C2_ER_IRQHandler(void) +{ + i2cErrorHandler(I2CDEV_2); +} - // Last byte remaining - i2cBusState->state = I2C_STATE_R_TRANSFER_EQ1; - i2cBusState->timeout = currentTicksUs; - } - else if (i2cBusState->len < 3) { - // Shouldn't happen - abort - I2C_AcknowledgeConfig(I2Cx, DISABLE); - I2C_GenerateSTOP(I2Cx,ENABLE); - I2C_ReceiveData(I2Cx); - - i2cBusState->txnOk = false; - i2cBusState->timeout = currentTicksUs; - i2cBusState->state = I2C_STATE_STOPPING; - } - else { - // 4 or more extra bytes remaining - *i2cBusState->buf++ = I2C_ReceiveData(I2Cx); - i2cBusState->len--; +void I2C2_EV_IRQHandler(void) +{ + i2cEventHandler(I2CDEV_2); +} - // Restart timeout and stay in this state - i2cBusState->timeout = currentTicksUs; - } - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; +void I2C3_ER_IRQHandler(void) +{ + i2cErrorHandler(I2CDEV_3); +} - case I2C_STATE_W_ADDR: - I2C_AcknowledgeConfig(I2Cx, DISABLE); - I2C_Send7bitAddress(I2Cx, i2cBusState->addr, I2C_Direction_Transmitter); - i2cBusState->state = I2C_STATE_W_ADDR_WAIT; - i2cBusState->timeout = currentTicksUs; - FALLTHROUGH; - - case I2C_STATE_W_ADDR_WAIT: - if (I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED) != ERROR) { - // Special no-address case, skip address byte transmission - if (i2cBusState->reg == 0xFF && i2cBusState->allowRawAccess) { - i2cBusState->state = I2C_STATE_W_TRANSFER; - } - else { - i2cBusState->state = I2C_STATE_W_REGISTER; - } - } - else if (I2C_GetFlagStatus(I2Cx, I2C_FLAG_AF) != RESET) { - i2cBusState->state = I2C_STATE_NACK; - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; +void I2C3_EV_IRQHandler(void) +{ + i2cEventHandler(I2CDEV_3); +} - case I2C_STATE_W_REGISTER: /* Send Register address */ - I2C_SendData(I2Cx, i2cBusState->reg); - i2cBusState->state = I2C_STATE_W_TRANSFER_WAIT; - i2cBusState->timeout = currentTicksUs; - FALLTHROUGH; +// Reinitialise the peripheral and clock the bus free. Always returns false so it can be used as the failure result. +static bool i2cHandleHardwareFailure(I2CDevice device) +{ + i2cErrorCount++; + i2cInit(device); + return false; +} - case I2C_STATE_W_TRANSFER_WAIT: - if (I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED) != ERROR) { - i2cBusState->state = I2C_STATE_W_TRANSFER; - } - else if (I2C_GetFlagStatus(I2Cx, I2C_FLAG_AF) != RESET) { - i2cBusState->state = I2C_STATE_NACK; - } - else if ((currentTicksUs - i2cBusState->timeout) >= I2C_TIMEOUT) { - i2cBusState->state = I2C_STATE_BUS_ERROR; - } - break; +// Wait for the hardware to release a CR1 control bit (START / STOP). Safe to call from the handlers. +static bool i2cWaitBitRelease(I2C_TypeDef *I2Cx, uint32_t cr1Mask) +{ + const timeUs_t startUs = microsISR(); - case I2C_STATE_W_TRANSFER: - if (i2cBusState->len > 0) { - I2C_SendData(I2Cx, *i2cBusState->buf); - i2cBusState->buf++; - i2cBusState->len--; - i2cBusState->timeout = currentTicksUs; - i2cBusState->state = I2C_STATE_W_TRANSFER_WAIT; - } - else { - I2C_GenerateSTOP(I2Cx, ENABLE); - i2cBusState->timeout = currentTicksUs; - i2cBusState->txnOk = true; - i2cBusState->state = I2C_STATE_STOPPING; - } - break; + while (I2Cx->CR1 & cr1Mask) { + if (cmpTimeUs(microsISR(), startUs) >= I2C_BIT_RELEASE_TIMEOUT_US) { + return false; + } + } - case I2C_STATE_NACK: - I2C_GenerateSTOP(I2Cx, ENABLE); - I2C_ClearFlag(I2Cx, I2C_FLAG_AF); - i2cBusState->timeout = currentTicksUs; - i2cBusState->state = I2C_STATE_STOPPING; - break; + const timeUs_t waitedUs = microsISR() - startUs; + if (waitedUs > i2cStats.maxBitWaitUs) { + i2cStats.maxBitWaitUs = waitedUs; } + + return true; } void i2cSetSpeed(uint8_t speed) @@ -463,34 +212,30 @@ void i2cSetSpeed(uint8_t speed) } } -uint32_t i2cTimeoutUserCallback(void) -{ - i2cErrorCount++; - return false; -} - void i2cInit(I2CDevice device) { - if (device == I2CINVALID) + if (!i2cIsValidDevice(device)) { return; + } i2cDevice_t *i2c = &(i2cHardwareMap[device]); + i2cBusState_t *state = &busState[device]; IO_t scl = IOGetByTag(i2c->scl); IO_t sda = IOGetByTag(i2c->sda); RCC_ClockCmd(i2c->rcc, ENABLE); + I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR | I2C_IT_BUF, DISABLE); + IOInit(scl, OWNER_I2C, RESOURCE_I2C_SCL, RESOURCE_INDEX(device)); IOInit(sda, OWNER_I2C, RESOURCE_I2C_SDA, RESOURCE_INDEX(device)); -#ifdef STM32F4 + // Release a slave that may still be driving SDA after a reset in the middle of a transfer + i2cUnstick(scl, sda); + IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_I2C); IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_I2C); -#else - IOConfigGPIO(scl, IOCFG_I2C); - IOConfigGPIO(sda, IOCFG_I2C); -#endif I2C_DeInit(i2c->dev); @@ -526,9 +271,14 @@ void i2cInit(I2CDevice device) I2C_StretchClockCmd(i2c->dev, ENABLE); I2C_Cmd(i2c->dev, ENABLE); - busState[device].device = device; - busState[device].initialized = true; - busState[device].state = I2C_STATE_STOPPED; + // Event and error interrupts are enabled in the peripheral per transfer, the NVIC side stays on + NVIC_SetPriority((IRQn_Type)i2c->er_irq, NVIC_PRIO_I2C_ER); + NVIC_EnableIRQ((IRQn_Type)i2c->er_irq); + NVIC_SetPriority((IRQn_Type)i2c->ev_irq, NVIC_PRIO_I2C_EV); + NVIC_EnableIRQ((IRQn_Type)i2c->ev_irq); + + memset(state, 0, sizeof(*state)); + state->initialized = true; } uint16_t i2cGetErrorCounter(void) @@ -536,60 +286,352 @@ uint16_t i2cGetErrorCounter(void) return i2cErrorCount; } -static void i2cWaitForCompletion(I2CDevice device) +// Set up and start a transfer. Returns false if the bus is busy or the peripheral failed to start. +static bool i2cStartTransfer(I2CDevice device, uint8_t addr, uint8_t reg, bool allowRawAccess, bool reading, uint8_t len, uint8_t *readBuf, const uint8_t *writeBuf) +{ + if (!i2cIsValidDevice(device)) { + return false; + } + + i2cBusState_t *state = &busState[device]; + I2C_TypeDef *I2Cx = i2cHardwareMap[device].dev; + + if (!state->initialized || state->busy) { + return false; + } + + if (reading && len == 0) { + return false; // nothing to receive, the engine would run past the buffer + } + + const timeUs_t callStartUs = micros(); + i2cStats.startCr2 = I2Cx->CR2 & (I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR); + i2cStats.eventIrqs = 0; + i2cStats.startUs = callStartUs; + + state->addr = addr << 1; + state->reg = reg; + state->rawAccess = (reg == 0xFF) && allowRawAccess; + state->reading = reading; + state->bytes = len; + state->readBuf = readBuf; + state->writeBuf = writeBuf; + state->subaddressSent = false; + state->index = 0; + state->startUs = callStartUs; + state->error = false; + state->busy = true; + + if (!(I2Cx->CR2 & I2C_IT_EVT)) { // interrupts are off between transfers + if (!(I2Cx->CR1 & I2C_CR1_START)) { // ensure sending a start + const timeUs_t startUs = micros(); + while (I2Cx->CR1 & I2C_CR1_STOP) { // wait for any stop to finish sending + if (cmpTimeUs(micros(), startUs) >= I2C_TIMEOUT) { + return i2cHandleHardwareFailure(device); + } + } + I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job + } + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); // the handlers take it from here + } + + i2cStats.startCallUs = micros() - callStartUs; + i2cDebugPublish(); + + return true; +} + +// Reset the peripheral after a transfer that did not complete and record the failure for the transfer's owner +static void i2cRecoverStuckTransfer(I2CDevice device) +{ + const uint8_t stuckAddr = busState[device].addr >> 1; // i2cInit() clears the state + + i2cHandleHardwareFailure(device); + busState[device].error = true; + i2cAddrResultSet(&busAddrResults[device], stuckAddr, true); +} + +// Block until the bus is idle. Returns false if the transfer in progress had to be aborted. +static bool i2cWaitForIdle(I2CDevice device) { - do { - i2cStateMachine(&busState[device], micros()); - } while (busState[device].state != I2C_STATE_STOPPED); + i2cBusState_t *state = &busState[device]; + const timeUs_t startUs = micros(); + + while (state->busy) { + if (cmpTimeUs(micros(), startUs) >= I2C_TIMEOUT) { + i2cRecoverStuckTransfer(device); + return false; + } + } + + return true; } -bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, const uint8_t * data, bool allowRawAccess) +bool i2cBusy(I2CDevice device, uint8_t addr_, bool *error) { - // Don't try to access the non-initialized device - if (!busState[device].initialized) + if (!i2cIsValidDevice(device)) { + if (error) { + *error = true; + } return false; + } + + i2cBusState_t *state = &busState[device]; - // Set up write transaction - busState[device].addr = addr << 1; - busState[device].reg = reg; - busState[device].rw = I2C_TXN_WRITE; - busState[device].len = len; - busState[device].buf = CONST_CAST(uint8_t*, data); - busState[device].txnOk = false; - busState[device].state = I2C_STATE_STARTING; - busState[device].allowRawAccess = allowRawAccess; + if (state->busy && cmpTimeUs(micros(), state->startUs) >= I2C_TIMEOUT) { + // No completion interrupt within the timeout, the transfer is stuck - reset the peripheral + i2cRecoverStuckTransfer(device); + } + + // Only the owner of the transfer on the bus is pending, everybody else gets the outcome of their own last transfer + const bool pending = state->busy && (state->addr >> 1) == addr_; + + if (error) { + *error = !pending && i2cAddrResultFailed(&busAddrResults[device], addr_); + } - // Inject I2C_EVENT_START - i2cWaitForCompletion(device); + i2cDebugPublish(); + + return pending; +} + +bool i2cReadStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf, bool allowRawAccess) +{ + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, true, len, buf, NULL); +} - return busState[device].txnOk; +bool i2cWriteBufferStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) +{ + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, false, len_, NULL, data); } -bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data, bool allowRawAccess) +bool i2cWriteStart(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data, bool allowRawAccess) { - return i2cWriteBuffer(device, addr, reg, 1, &data, allowRawAccess); + if (!i2cIsValidDevice(device) || busState[device].busy) { + return false; // don't touch txByte while it may still be going out + } + + busState[device].txByte = data; + return i2cStartTransfer(device, addr_, reg_, allowRawAccess, false, 1, NULL, &busState[device].txByte); } -bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t* buf, bool allowRawAccess) +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) { - // Don't try to access the non-initialized device - if (!busState[device].initialized) + if (!i2cIsValidDevice(device) || !busState[device].initialized) { + return false; + } + + // Let a non-blocking transfer of another device finish first + if (!i2cWaitForIdle(device)) { + return false; + } + + if (!i2cStartTransfer(device, addr_, reg_, allowRawAccess, false, len_, NULL, data)) { return false; + } - // Set up read transaction - busState[device].addr = addr << 1; - busState[device].reg = reg; - busState[device].rw = I2C_TXN_READ; - busState[device].len = len; - busState[device].buf = buf; - busState[device].txnOk = false; - busState[device].state = I2C_STATE_STARTING; - busState[device].allowRawAccess = allowRawAccess; + return i2cWaitForIdle(device) && !busState[device].error; +} - // Inject I2C_EVENT_START - i2cWaitForCompletion(device); +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data, bool allowRawAccess) +{ + return i2cWriteBuffer(device, addr_, reg_, 1, &data, allowRawAccess); +} - return busState[device].txnOk; +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf, bool allowRawAccess) +{ + if (!i2cIsValidDevice(device) || !busState[device].initialized) { + return false; + } + + // Let a non-blocking transfer of another device finish first + if (!i2cWaitForIdle(device)) { + return false; + } + + if (!i2cStartTransfer(device, addr_, reg_, allowRawAccess, true, len, buf, NULL)) { + return false; + } + + return i2cWaitForIdle(device) && !busState[device].error; +} + +static void i2cErrorHandler(I2CDevice device) +{ + I2C_TypeDef *I2Cx = i2cHardwareMap[device].dev; + i2cBusState_t *state = &busState[device]; + + const uint32_t SR1Register = I2Cx->SR1; + const uint8_t addr = state->addr >> 1; // the owner, a reset below would clear it + + i2cStats.errorIrqs++; + + if (SR1Register & (I2C_SR1_BERR | I2C_SR1_ARLO | I2C_SR1_AF | I2C_SR1_OVR)) { + state->error = true; + i2cErrorCount++; // keep the "I2C Errors" count of status / MSP meaningful for non-blocking transfers + } + + // If AF, BERR or ARLO, abandon the current job + if (SR1Register & (I2C_SR1_BERR | I2C_SR1_ARLO | I2C_SR1_AF)) { + (void)I2Cx->SR2; // read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK) + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully) + if (!(SR1Register & I2C_SR1_ARLO) && !(I2Cx->CR1 & I2C_CR1_STOP)) { // if we dont have an ARLO error, ensure sending of a stop (after ARLO the hardware has already released the bus) + if (I2Cx->CR1 & I2C_CR1_START) { // We are currently trying to send a start, this is very bad as start, stop will hang the peripheral + i2cWaitBitRelease(I2Cx, I2C_CR1_START); // wait for any start to finish sending + I2C_GenerateSTOP(I2Cx, ENABLE); // send stop to finalise bus transaction + i2cWaitBitRelease(I2Cx, I2C_CR1_STOP); // wait for stop to finish sending + i2cHandleHardwareFailure(device); // reset and configure the hardware + state->error = true; // the reset cleared the state, the transfer still failed + } + else { + I2C_GenerateSTOP(I2Cx, ENABLE); // stop to free up the bus + } + } + } + + // The bus is idle on every path out of here. Leaving EVT enabled would make the next i2cStartTransfer() skip + // generating START and the transfer would only fail through the timeout + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR | I2C_IT_BUF, DISABLE); + I2Cx->SR1 &= ~(I2C_SR1_BERR | I2C_SR1_ARLO | I2C_SR1_AF | I2C_SR1_OVR); // reset all the error bits to clear the interrupt + i2cAddrResultSet(&busAddrResults[device], addr, state->error); + state->busy = false; +} + +static void i2cEventHandlerBody(I2CDevice device) +{ + I2C_TypeDef *I2Cx = i2cHardwareMap[device].dev; + i2cBusState_t *state = &busState[device]; + + // Only the low byte carries the event flags, the error flags in the high byte are handled by the error handler + const uint8_t SReg_1 = I2Cx->SR1; + + if (SReg_1 & I2C_SR1_SB) { // we just sent a start - EV5 in ref manual + I2Cx->CR1 &= ~I2C_CR1_POS; // reset the POS bit so ACK/NACK applied to the current byte + I2C_AcknowledgeConfig(I2Cx, ENABLE); // make sure ACK is on + state->index = 0; // reset the index + if (state->reading && (state->subaddressSent || state->rawAccess)) { // we have sent the subaddr + state->subaddressSent = true; // make sure this is set in case of no subaddress, so following code runs correctly + if (state->bytes == 2) { + I2Cx->CR1 |= I2C_CR1_POS; // set the POS bit so NACK applied to the final byte in the two byte read + } + I2C_Send7bitAddress(I2Cx, state->addr, I2C_Direction_Receiver); // send the address and set hardware mode + } + else { // direction is Tx, or we havent sent the sub and rep start + I2C_Send7bitAddress(I2Cx, state->addr, I2C_Direction_Transmitter); // send the address and set hardware mode + if (!state->rawAccess) { + state->index = -1; // send a subaddress + } + } + } + else if (SReg_1 & I2C_SR1_ADDR) { // we just sent the address - EV6 in ref manual + // Read SR1,2 to clear ADDR + __DMB(); // memory fence to control hardware + if (state->bytes == 1 && state->reading && state->subaddressSent) { // we are receiving 1 byte - EV6_3 + I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK + __DMB(); + (void)I2Cx->SR2; // clear ADDR after ACK is turned off + I2C_GenerateSTOP(I2Cx, ENABLE); // program the stop + I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // allow us to have an EV7 + } + else { // EV6 and EV6_1 + (void)I2Cx->SR2; // clear the ADDR here + __DMB(); + if (state->bytes == 2 && state->reading && state->subaddressSent) { // rx 2 bytes - EV6_1 + I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to fill + } + else if (state->bytes == 3 && state->reading && state->subaddressSent) { // rx 3 bytes + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // make sure RXNE disabled so we get a BTF in two bytes time + } + else { // receiving greater than three bytes, sending subaddress, or transmitting + I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); + } + } + } + else if (SReg_1 & I2C_SR1_BTF) { // Byte transfer finished - EV7_2, EV7_3 or EV8_2 + if (state->reading && state->subaddressSent) { // EV7_2, EV7_3 + if (state->bytes > 2) { // EV7_2 + I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK + state->readBuf[state->index++] = (uint8_t)I2Cx->DR; // read data N-2 + I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop + state->readBuf[state->index++] = (uint8_t)I2Cx->DR; // read data N-1 + I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // enable TXE to allow the final EV7 + } + else { // EV7_3 + I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop + state->readBuf[state->index++] = (uint8_t)I2Cx->DR; // read data N-1 + state->readBuf[state->index++] = (uint8_t)I2Cx->DR; // read data N + state->index++; // to show job completed + } + } + else { // EV8_2, which may be due to a subaddress sent or a write completion + if (state->subaddressSent || !state->reading) { + I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop + state->index++; // to show that the job is complete + } + else { // We need to send a subaddress + I2C_GenerateSTART(I2Cx, ENABLE); // program the repeated Start + state->subaddressSent = true; // this is set back to zero upon completion of the current task + } + } + // we must wait for the start to clear, otherwise we get constant BTF + if (!i2cWaitBitRelease(I2Cx, I2C_CR1_START)) { + i2cRecoverStuckTransfer(device); + return; + } + } + else if (SReg_1 & I2C_SR1_RXNE) { // Byte received - EV7 + state->readBuf[state->index++] = (uint8_t)I2Cx->DR; + if (state->bytes == (state->index + 3)) { + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush so we can get an EV7_2 + } + if (state->bytes == state->index) { // We have completed a final EV7 + state->index++; // to show job is complete + } + } + else if (SReg_1 & I2C_SR1_TXE) { // Byte transmitted EV8 / EV8_1 + if (state->index != -1) { // we dont have a subaddress to send + if (state->index < state->bytes) { + I2Cx->DR = state->writeBuf[state->index++]; + if (state->bytes == state->index) { // we have sent all the data + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush + } + } + else { // raw write without data - nothing to send, end the transfer + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); + I2C_GenerateSTOP(I2Cx, ENABLE); + state->index = state->bytes + 1; // to show that the job is complete + } + } + else { + state->index++; + I2Cx->DR = state->reg; // send the subaddress + if (state->reading || !(state->bytes)) { // if receiving or sending 0 bytes, flush now + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush + } + } + } + + if (state->index == state->bytes + 1) { // we have completed the current job + state->subaddressSent = false; // reset this here + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // bus is inactive, disable interrupts to prevent BTF + i2cStats.lastTransferUs = microsISR() - i2cStats.startUs; + i2cAddrResultSet(&busAddrResults[device], state->addr >> 1, false); + state->busy = false; + } +} + +static void i2cEventHandler(I2CDevice device) +{ + const timeUs_t entryUs = microsISR(); + + i2cStats.eventIrqs++; + i2cEventHandlerBody(device); + + const timeUs_t spentUs = microsISR() - entryUs; + if (spentUs > i2cStats.maxIrqUs) { + i2cStats.maxIrqUs = spentUs; + } } static void i2cUnstick(IO_t scl, IO_t sda) diff --git a/src/main/drivers/compass/compass.h b/src/main/drivers/compass/compass.h index 655021c7709..dd3fe52a13d 100644 --- a/src/main/drivers/compass/compass.h +++ b/src/main/drivers/compass/compass.h @@ -21,10 +21,14 @@ #include "drivers/sensor.h" +// Non-blocking read step, see busReadStepResult_e in drivers/bus.h +typedef busReadStepResult_e (*sensorMagReadStartFuncPtr)(struct magDev_s *mag, bool firstStep); + typedef struct magDev_s { busDevice_t * busDev; sensorMagInitFuncPtr init; // initialize function - sensorMagReadFuncPtr read; // read 3 axis data function + sensorMagReadStartFuncPtr readStart; // optional: start the next non-blocking transfer of a sample, read() then only parses + sensorMagReadFuncPtr read; // read 3 axis data function (blocking when readStart is NULL) struct { bool useExternal; union { diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 58d100d9f71..8cd46fb864b 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -115,32 +115,56 @@ static int16_t parseMag(uint8_t *raw, int16_t gain) { return constrain(ret, INT16_MIN, INT16_MAX); } -static bool ak8963Read(magDev_t * mag) -{ - bool ack = false; - uint8_t buf[7]; +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t ak8963Status; +static uint8_t ak8963Data[7]; // HXL..HZH followed by STATUS2 - ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS1, &buf[0]); +// Three transfers per sample: status, data once DRDY is set, then the trigger of the next single measurement +static busReadStepResult_e ak8963ReadStart(magDev_t * mag, bool firstStep) +{ + static uint8_t step = 0; - if (!ack || (buf[0] & STATUS1_DATA_READY) == 0) { - return false; + if (firstStep) { + step = 0; + ak8963Status = 0; } - ack = busReadBuf(mag->busDev, AK8963_MAG_REG_HXL, &buf[0], 7); - - if (!ack) { - return false; + switch (step) { + case 0: + if (!busReadBufStart(mag->busDev, AK8963_MAG_REG_STATUS1, &ak8963Status, 1)) { + return BUS_READ_STEP_BUSY; + } + step = 1; + return BUS_READ_STEP_NEXT; + + case 1: + if ((ak8963Status & STATUS1_DATA_READY) == 0) { + return BUS_READ_STEP_LAST; // measurement not finished, ak8963Read() reports the miss + } + if (!busReadBufStart(mag->busDev, AK8963_MAG_REG_HXL, ak8963Data, sizeof(ak8963Data))) { + return BUS_READ_STEP_BUSY; + } + step = 2; + return BUS_READ_STEP_NEXT; + + default: + // Data is in, trigger the next single measurement; ak8963Read() parses once this write is out + if (!busWriteStart(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE)) { + return BUS_READ_STEP_BUSY; + } + return BUS_READ_STEP_LAST; } +} - ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); - - if (buf[6] & STATUS2_MAG_SENSOR_OVERFLOW) { +static bool ak8963Read(magDev_t * mag) +{ + if ((ak8963Status & STATUS1_DATA_READY) == 0 || (ak8963Data[6] & STATUS2_MAG_SENSOR_OVERFLOW)) { return false; } - mag->magADCRaw[X] = -parseMag(buf + 0, magGain[X]); - mag->magADCRaw[Y] = parseMag(buf + 2, magGain[Y]); - mag->magADCRaw[Z] = -parseMag(buf + 4, magGain[Z]); + mag->magADCRaw[X] = -parseMag(ak8963Data + 0, magGain[X]); + mag->magADCRaw[Y] = parseMag(ak8963Data + 2, magGain[Y]); + mag->magADCRaw[Z] = -parseMag(ak8963Data + 4, magGain[Z]); return true; } @@ -175,6 +199,7 @@ bool ak8963Detect(magDev_t * mag) } mag->init = ak8963Init; + mag->readStart = ak8963ReadStart; mag->read = ak8963Read; return true; diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index c30869c613a..6d5692c3ca4 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/src/main/drivers/compass/compass_ak8975.c @@ -95,33 +95,74 @@ static bool ak8975Init(magDev_t * mag) #define BIT_STATUS2_REG_DATA_ERROR (1 << 2) #define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW (1 << 3) -static bool ak8975Read(magDev_t * mag) +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t ak8975Status1; +static uint8_t ak8975Status2; +static uint8_t ak8975Data[6]; + +// Four transfers per sample: status, data once DRDY is set, STATUS2, then the trigger of the next measurement +static busReadStepResult_e ak8975ReadStart(magDev_t * mag, bool firstStep) { - uint8_t status; - uint8_t buf[6]; + static uint8_t step = 0; + + if (firstStep) { + step = 0; + ak8975Status1 = 0; + } + + switch (step) { + case 0: + if (!busReadBufStart(mag->busDev, AK8975_MAG_REG_STATUS1, &ak8975Status1, 1)) { + return BUS_READ_STEP_BUSY; + } + step = 1; + return BUS_READ_STEP_NEXT; + + case 1: + if ((ak8975Status1 & BIT_STATUS1_REG_DATA_READY) == 0) { + return BUS_READ_STEP_LAST; // measurement not finished, ak8975Read() reports the miss + } + if (!busReadBufStart(mag->busDev, AK8975_MAG_REG_HXL, ak8975Data, sizeof(ak8975Data))) { // AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH + return BUS_READ_STEP_BUSY; + } + step = 2; + return BUS_READ_STEP_NEXT; + + case 2: + if (!busReadBufStart(mag->busDev, AK8975_MAG_REG_STATUS2, &ak8975Status2, 1)) { + return BUS_READ_STEP_BUSY; + } + step = 3; + return BUS_READ_STEP_NEXT; + + default: + // Start the next measurement; ak8975Read() parses once this write is out + if (!busWriteStart(mag->busDev, AK8975_MAG_REG_CNTL, 0x01)) { + return BUS_READ_STEP_BUSY; + } + return BUS_READ_STEP_LAST; + } +} +static bool ak8975Read(magDev_t * mag) +{ // set magData to zero for case of failed read mag->magADCRaw[X] = 0; mag->magADCRaw[Y] = 0; mag->magADCRaw[Z] = 0; - bool ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS1, &status); - if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) { + if ((ak8975Status1 & BIT_STATUS1_REG_DATA_READY) == 0) { return false; } - ack = busReadBuf(mag->busDev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH - ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS2, &status); - - if (!ack || (status & BIT_STATUS2_REG_DATA_ERROR) || (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW)) { + if ((ak8975Status2 & BIT_STATUS2_REG_DATA_ERROR) || (ak8975Status2 & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW)) { return false; } - mag->magADCRaw[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4; - mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * 4; - mag->magADCRaw[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4; + mag->magADCRaw[X] = -(int16_t)(ak8975Data[1] << 8 | ak8975Data[0]) * 4; + mag->magADCRaw[Y] = (int16_t)(ak8975Data[3] << 8 | ak8975Data[2]) * 4; + mag->magADCRaw[Z] = -(int16_t)(ak8975Data[5] << 8 | ak8975Data[4]) * 4; - ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x01); // start reading again return true; } @@ -155,6 +196,7 @@ bool ak8975Detect(magDev_t * mag) } mag->init = ak8975Init; + mag->readStart = ak8975ReadStart; mag->read = ak8975Read; return true; diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index 6bbd20c189c..0b5a784adb0 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/src/main/drivers/compass/compass_hmc5883l.c @@ -111,28 +111,24 @@ #define HMC_POS_BIAS 1 #define HMC_NEG_BIAS 2 -static bool hmc5883lRead(magDev_t * mag) +// Target of the non-blocking data read, filled by the bus driver in the background +static uint8_t hmc5883lData[6]; + +static busReadStepResult_e hmc5883lReadStart(magDev_t * mag, bool firstStep) { - uint8_t buf[6]; - bool ack; + UNUSED(firstStep); - if (mag->busDev->busType == BUSTYPE_SPI) { - ack = busReadBuf(mag->busDev, MAG_DATA_REGISTER_SPI, buf, 6); - } - else { - ack = busReadBuf(mag->busDev, MAG_DATA_REGISTER, buf, 6); - } + // Continuous conversion mode, the data registers always hold the latest sample + const uint8_t dataRegister = (mag->busDev->busType == BUSTYPE_SPI) ? MAG_DATA_REGISTER_SPI : MAG_DATA_REGISTER; - if (!ack) { - mag->magADCRaw[X] = 0; - mag->magADCRaw[Y] = 0; - mag->magADCRaw[Z] = 0; - return false; - } + return busReadBufStart(mag->busDev, dataRegister, hmc5883lData, sizeof(hmc5883lData)) ? BUS_READ_STEP_LAST : BUS_READ_STEP_BUSY; +} - mag->magADCRaw[X] = (int16_t)(buf[0] << 8 | buf[1]); - mag->magADCRaw[Z] = (int16_t)(buf[2] << 8 | buf[3]); - mag->magADCRaw[Y] = (int16_t)(buf[4] << 8 | buf[5]); +static bool hmc5883lRead(magDev_t * mag) +{ + mag->magADCRaw[X] = (int16_t)(hmc5883lData[0] << 8 | hmc5883lData[1]); + mag->magADCRaw[Z] = (int16_t)(hmc5883lData[2] << 8 | hmc5883lData[3]); + mag->magADCRaw[Y] = (int16_t)(hmc5883lData[4] << 8 | hmc5883lData[5]); return true; } @@ -182,6 +178,7 @@ bool hmc5883lDetect(magDev_t * mag) } mag->init = hmc5883lInit; + mag->readStart = hmc5883lReadStart; mag->read = hmc5883lRead; return true; diff --git a/src/main/drivers/compass/compass_ist8308.c b/src/main/drivers/compass/compass_ist8308.c index 81a029debe8..56fc90b1ce6 100644 --- a/src/main/drivers/compass/compass_ist8308.c +++ b/src/main/drivers/compass/compass_ist8308.c @@ -107,29 +107,50 @@ static bool ist8308Init(magDev_t * mag) return true; } -static bool ist8308Read(magDev_t * mag) +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t ist8308Status; +static uint8_t ist8308Data[6]; + +// Two transfers per sample: the status byte first, the data only when DRDY confirms a fresh sample +static busReadStepResult_e ist8308ReadStart(magDev_t * mag, bool firstStep) { - const float LSB2FSV = 1.5; // 1.5mG + static bool statusStarted = false; - uint8_t status = 0; + if (firstStep) { + statusStarted = false; + ist8308Status = 0; + } - if (!busRead(mag->busDev, IST8308_REG_STAT1, &status)) { - return false; + if (!statusStarted) { + if (!busReadBufStart(mag->busDev, IST8308_REG_STAT1, &ist8308Status, 1)) { + return BUS_READ_STEP_BUSY; + } + statusStarted = true; + return BUS_READ_STEP_NEXT; } // The IST8308 updates its data registers once per output-data period. Only read // them once DRDY is set; a read racing an update would mix old and new samples - // across axes, corrupting the mag vector and calibration. When no fresh sample - // is ready, return success so the caller keeps the previous values. - if (!(status & IST8308_DRDY_MASK)) { - return true; + // across axes, corrupting the mag vector and calibration. + if ((ist8308Status & IST8308_DRDY_MASK) == 0) { + return BUS_READ_STEP_LAST; // nothing new to fetch, ist8308Read() keeps the previous sample + } + + if (!busReadBufStart(mag->busDev, IST8308_REG_DATA, ist8308Data, sizeof(ist8308Data))) { + return BUS_READ_STEP_BUSY; } - uint8_t buf[6]; + return BUS_READ_STEP_LAST; +} - bool ack = busReadBuf(mag->busDev, IST8308_REG_DATA, buf, 6); - if (!ack) { - return false; +static bool ist8308Read(magDev_t * mag) +{ + const float LSB2FSV = 1.5; // 1.5mG + const uint8_t *buf = ist8308Data; + + // When no fresh sample was ready, return success so the caller keeps the previous values + if ((ist8308Status & IST8308_DRDY_MASK) == 0) { + return true; } // Invert Y axis to co convert from left to right coordinate system @@ -170,6 +191,7 @@ bool ist8308Detect(magDev_t * mag) } mag->init = ist8308Init; + mag->readStart = ist8308ReadStart; mag->read = ist8308Read; return true; diff --git a/src/main/drivers/compass/compass_ist8310.c b/src/main/drivers/compass/compass_ist8310.c index 85476d318e8..8d59ea34362 100644 --- a/src/main/drivers/compass/compass_ist8310.c +++ b/src/main/drivers/compass/compass_ist8310.c @@ -126,28 +126,50 @@ static bool ist8310Init(magDev_t * mag) return true; } -static bool ist8310Read(magDev_t * mag) +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t ist8310Status; +static uint8_t ist8310Data[6]; + +// Two transfers per sample: the status byte first, the data only when DRDY confirms a fresh sample +static busReadStepResult_e ist8310ReadStart(magDev_t * mag, bool firstStep) { - uint8_t status = 0; + static bool statusStarted = false; - if (!busRead(mag->busDev, IST8310_REG_STAT1, &status)) { - return false; + if (firstStep) { + statusStarted = false; + ist8310Status = 0; + } + + if (!statusStarted) { + if (!busReadBufStart(mag->busDev, IST8310_REG_STAT1, &ist8310Status, 1)) { + return BUS_READ_STEP_BUSY; + } + statusStarted = true; + return BUS_READ_STEP_NEXT; } // The IST8310 updates its data registers once per output-data period. Only read // them once DRDY is set; a read racing an update would mix old and new samples - // across axes, corrupting the mag vector and calibration. When no fresh sample - // is ready, return success so the caller keeps the previous values. - if (!(status & IST8310_DRDY_MASK)) { - return true; + // across axes, corrupting the mag vector and calibration. + if ((ist8310Status & IST8310_DRDY_MASK) == 0) { + return BUS_READ_STEP_LAST; // nothing new to fetch, ist8310Read() keeps the previous sample } - uint8_t buf[6]; + if (!busReadBufStart(mag->busDev, IST8310_REG_DATA, ist8310Data, sizeof(ist8310Data))) { + return BUS_READ_STEP_BUSY; + } + + return BUS_READ_STEP_LAST; +} + +static bool ist8310Read(magDev_t * mag) +{ + const uint8_t *buf = ist8310Data; uint8_t LSB2FSV = 3; // 3mG - 14 bit - bool ack = busReadBuf(mag->busDev, IST8310_REG_DATA, buf, 6); - if (!ack) { - return false; + // When no fresh sample was ready, return success so the caller keeps the previous values + if ((ist8310Status & IST8310_DRDY_MASK) == 0) { + return true; } // Invert Y axis to co convert from left to right coordinate system @@ -185,6 +207,7 @@ bool ist8310Detect(magDev_t * mag) if (deviceDetect(mag)) { mag->init = ist8310Init; + mag->readStart = ist8310ReadStart; mag->read = ist8310Read; return true; } else { diff --git a/src/main/drivers/compass/compass_lis2mdl.c b/src/main/drivers/compass/compass_lis2mdl.c index 625d0c4de50..ca74818af11 100644 --- a/src/main/drivers/compass/compass_lis2mdl.c +++ b/src/main/drivers/compass/compass_lis2mdl.c @@ -22,6 +22,7 @@ #ifdef USE_MAG_LIS2MDL +#include "common/utils.h" #include "common/axis.h" #include "drivers/time.h" @@ -66,22 +67,48 @@ static bool lis2mdlInit(magDev_t *mag) return ack; } +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t lis2mdlStatus; +static uint8_t lis2mdlData[6]; + +// Two transfers per sample: the status byte first, the data only when ZYXDA confirms a fresh sample +static busReadStepResult_e lis2mdlReadStart(magDev_t *mag, bool firstStep) +{ + static bool statusStarted = false; + + if (firstStep) { + statusStarted = false; + lis2mdlStatus = 0; + } + + if (!statusStarted) { + if (!busReadBufStart(mag->busDev, LIS2MDL_REG_STATUS_REG, &lis2mdlStatus, 1)) { + return BUS_READ_STEP_BUSY; + } + statusStarted = true; + return BUS_READ_STEP_NEXT; + } + + if (!(lis2mdlStatus & LIS2MDL_STATUS_ZYXDA)) { + return BUS_READ_STEP_LAST; // nothing new to fetch, lis2mdlRead() reports the miss + } + + if (!busReadBufStart(mag->busDev, LIS2MDL_REG_OUTX_L, lis2mdlData, sizeof(lis2mdlData))) { + return BUS_READ_STEP_BUSY; + } + + return BUS_READ_STEP_LAST; +} + static bool lis2mdlRead(magDev_t *mag) { - uint8_t status = 0; - uint8_t buf[6]; + const uint8_t *buf = lis2mdlData; mag->magADCRaw[X] = 0; mag->magADCRaw[Y] = 0; mag->magADCRaw[Z] = 0; - bool ack = busRead(mag->busDev, LIS2MDL_REG_STATUS_REG, &status); - if (!ack || !(status & LIS2MDL_STATUS_ZYXDA)) { - return false; - } - - ack = busReadBuf(mag->busDev, LIS2MDL_REG_OUTX_L, buf, 6); - if (!ack) { + if (!(lis2mdlStatus & LIS2MDL_STATUS_ZYXDA)) { return false; } @@ -127,6 +154,7 @@ bool lis2mdlDetect(magDev_t *mag) } mag->init = lis2mdlInit; + mag->readStart = lis2mdlReadStart; mag->read = lis2mdlRead; return true; diff --git a/src/main/drivers/compass/compass_lis3mdl.c b/src/main/drivers/compass/compass_lis3mdl.c index 47cbf9c0332..705b8aedc98 100644 --- a/src/main/drivers/compass/compass_lis3mdl.c +++ b/src/main/drivers/compass/compass_lis3mdl.c @@ -24,6 +24,7 @@ #ifdef USE_MAG_LIS3MDL +#include "common/utils.h" #include "common/axis.h" #include "drivers/time.h" @@ -104,22 +105,46 @@ #define LIS3MDL_FAST_READ 0x80 // Default 0 #define LIS3MDL_BDU 0x40 // Default 0 -static bool lis3mdlRead(magDev_t * mag) +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t lis3mdlStatus; +static uint8_t lis3mdlData[6]; + +#define LIS3MDL_STATUS_ZYXDA 0x08 + +// Two transfers per sample: the status byte first, the data only when ZYXDA confirms a fresh sample +static busReadStepResult_e lis3mdlReadStart(magDev_t * mag, bool firstStep) { - bool ack; - uint8_t status = 0; + static bool statusStarted = false; - ack = busRead(mag->busDev, LIS3MDL_REG_STATUS_REG, &status); + if (firstStep) { + statusStarted = false; + lis3mdlStatus = 0; + } - if ( !( ack && ( ( status & 0x08 ) >> 3 ) ) ) { - return false; - } + if (!statusStarted) { + if (!busReadBufStart(mag->busDev, LIS3MDL_REG_STATUS_REG, &lis3mdlStatus, 1)) { + return BUS_READ_STEP_BUSY; + } + statusStarted = true; + return BUS_READ_STEP_NEXT; + } - uint8_t buf[6]; + if (!(lis3mdlStatus & LIS3MDL_STATUS_ZYXDA)) { + return BUS_READ_STEP_LAST; // nothing new to fetch, lis3mdlRead() reports the miss + } - ack = busReadBuf(mag->busDev, LIS3MDL_REG_OUT_X_L, buf, 6); + if (!busReadBufStart(mag->busDev, LIS3MDL_REG_OUT_X_L, lis3mdlData, sizeof(lis3mdlData))) { + return BUS_READ_STEP_BUSY; + } + + return BUS_READ_STEP_LAST; +} + +static bool lis3mdlRead(magDev_t * mag) +{ + const uint8_t *buf = lis3mdlData; - if (!ack) { + if (!(lis3mdlStatus & LIS3MDL_STATUS_ZYXDA)) { mag->magADCRaw[X] = 0; mag->magADCRaw[Y] = 0; mag->magADCRaw[Z] = 0; @@ -176,6 +201,7 @@ bool lis3mdlDetect(magDev_t * mag) } mag->init = lis3mdlInit; + mag->readStart = lis3mdlReadStart; mag->read = lis3mdlRead; return true; diff --git a/src/main/drivers/compass/compass_mag3110.c b/src/main/drivers/compass/compass_mag3110.c index 5cd8454ebc8..7be729cd34a 100644 --- a/src/main/drivers/compass/compass_mag3110.c +++ b/src/main/drivers/compass/compass_mag3110.c @@ -75,23 +75,49 @@ static bool mag3110Init(magDev_t * mag) #define BIT_STATUS_REG_DATA_READY (1 << 3) +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t mag3110Status; +static uint8_t mag3110Data[6]; + +// Two transfers per sample: the status byte first, the data only when DRDY confirms a fresh sample +static busReadStepResult_e mag3110ReadStart(magDev_t * mag, bool firstStep) +{ + static bool statusStarted = false; + + if (firstStep) { + statusStarted = false; + mag3110Status = 0; + } + + if (!statusStarted) { + if (!busReadBufStart(mag->busDev, MAG3110_MAG_REG_STATUS, &mag3110Status, 1)) { + return BUS_READ_STEP_BUSY; + } + statusStarted = true; + return BUS_READ_STEP_NEXT; + } + + if ((mag3110Status & BIT_STATUS_REG_DATA_READY) == 0) { + return BUS_READ_STEP_LAST; // nothing new to fetch, mag3110Read() reports the miss + } + + if (!busReadBufStart(mag->busDev, MAG3110_MAG_REG_HXL, mag3110Data, sizeof(mag3110Data))) { + return BUS_READ_STEP_BUSY; + } + + return BUS_READ_STEP_LAST; +} + static bool mag3110Read(magDev_t * mag) { - uint8_t status; - uint8_t buf[6]; + const uint8_t *buf = mag3110Data; // set magData to zero for case of failed read mag->magADCRaw[X] = 0; mag->magADCRaw[Y] = 0; mag->magADCRaw[Z] = 0; - bool ack = busRead(mag->busDev, MAG3110_MAG_REG_STATUS, &status); - if (!ack || (status & BIT_STATUS_REG_DATA_READY) == 0) { - return false; - } - - ack = busReadBuf(mag->busDev, MAG3110_MAG_REG_HXL, buf, 6); - if (!ack) { + if ((mag3110Status & BIT_STATUS_REG_DATA_READY) == 0) { return false; } @@ -132,6 +158,7 @@ bool mag3110detect(magDev_t * mag) } mag->init = mag3110Init; + mag->readStart = mag3110ReadStart; mag->read = mag3110Read; return true; diff --git a/src/main/drivers/compass/compass_mlx90393.c b/src/main/drivers/compass/compass_mlx90393.c index 607d2bf6d35..582c7bccd09 100644 --- a/src/main/drivers/compass/compass_mlx90393.c +++ b/src/main/drivers/compass/compass_mlx90393.c @@ -100,21 +100,26 @@ static void mlx90393WriteRegister(magDev_t * mag, uint8_t reg_val, uint16_t valu } // ======================================================================================= -static bool mlx90393Read(magDev_t * mag) +// Target of the non-blocking measurement read, filled by the bus driver in the background +static uint8_t mlx90393Data[7]; + +static busReadStepResult_e mlx90393ReadStart(magDev_t * mag, bool firstStep) { + UNUSED(firstStep); - uint8_t buf[7] = {0}; + // Burst mode, the read measurement command returns the status byte and the latest X, Y, Z + return busReadBufStart(mag->busDev, MLX90393_READ_MEASUREMENT | MLX90393_MEASURE_3D, mlx90393Data, sizeof(mlx90393Data)) ? BUS_READ_STEP_LAST : BUS_READ_STEP_BUSY; +} - if (!busReadBuf(mag->busDev, MLX90393_READ_MEASUREMENT | MLX90393_MEASURE_3D, buf, sizeof(buf))) { - return false; - } +static bool mlx90393Read(magDev_t * mag) +{ + const uint8_t *buf = mlx90393Data; mag->magADCRaw[X] = ((short)(buf[1] << 8 | buf[2])); mag->magADCRaw[Y] = ((short)(buf[3] << 8 | buf[4])); mag->magADCRaw[Z] = ((short)(buf[5] << 8 | buf[6])); return true; - } static bool deviceDetect(magDev_t * mag) @@ -169,6 +174,7 @@ bool mlx90393Detect(magDev_t * mag) } mag->init = mlx90393Init; + mag->readStart = mlx90393ReadStart; mag->read = mlx90393Read; return true; diff --git a/src/main/drivers/compass/compass_qmc5883l.c b/src/main/drivers/compass/compass_qmc5883l.c index aca7b1ef995..12d746eb720 100644 --- a/src/main/drivers/compass/compass_qmc5883l.c +++ b/src/main/drivers/compass/compass_qmc5883l.c @@ -76,6 +76,7 @@ #define QMC5883L_REG_DATA_OUTPUT_X 0x00 #define QMC5883L_REG_STATUS 0x06 +#define QMC5883L_STATUS_DRDY 0x01 #define QMC5883L_REG_ID 0x0D #define QMC5883_ID_VAL 0xFF @@ -92,29 +93,52 @@ static bool qmc5883Init(magDev_t * mag) return ack; } -static bool qmc5883Read(magDev_t * mag) +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t qmc5883Status; +static uint8_t qmc5883Data[6]; + +// Two transfers per sample: the status byte first, the data only when DRDY confirms a fresh sample +static busReadStepResult_e qmc5883ReadStart(magDev_t * mag, bool firstStep) { - uint8_t status; - uint8_t buf[6]; + static bool statusStarted = false; - // set magData to zero for case of failed read - mag->magADCRaw[X] = 0; - mag->magADCRaw[Y] = 0; - mag->magADCRaw[Z] = 0; + if (firstStep) { + statusStarted = false; + qmc5883Status = 0; + } - bool ack = busRead(mag->busDev, QMC5883L_REG_STATUS, &status); - if (!ack || (status & 0x01) == 0) { - return false; + if (!statusStarted) { + if (!busReadBufStart(mag->busDev, QMC5883L_REG_STATUS, &qmc5883Status, 1)) { + return BUS_READ_STEP_BUSY; + } + statusStarted = true; + return BUS_READ_STEP_NEXT; + } + + if ((qmc5883Status & QMC5883L_STATUS_DRDY) == 0) { + return BUS_READ_STEP_LAST; // nothing new to fetch, qmc5883Read() reports the miss } - ack = busReadBuf(mag->busDev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6); - if (!ack) { + if (!busReadBufStart(mag->busDev, QMC5883L_REG_DATA_OUTPUT_X, qmc5883Data, sizeof(qmc5883Data))) { + return BUS_READ_STEP_BUSY; + } + + return BUS_READ_STEP_LAST; +} + +static bool qmc5883Read(magDev_t * mag) +{ + if ((qmc5883Status & QMC5883L_STATUS_DRDY) == 0) { + // set magData to zero for case of failed read + mag->magADCRaw[X] = 0; + mag->magADCRaw[Y] = 0; + mag->magADCRaw[Z] = 0; return false; } - mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]); - mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]); - mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]); + mag->magADCRaw[X] = (int16_t)(qmc5883Data[1] << 8 | qmc5883Data[0]); + mag->magADCRaw[Y] = (int16_t)(qmc5883Data[3] << 8 | qmc5883Data[2]); + mag->magADCRaw[Z] = (int16_t)(qmc5883Data[5] << 8 | qmc5883Data[4]); return true; } @@ -157,6 +181,7 @@ bool qmc5883Detect(magDev_t * mag) } mag->init = qmc5883Init; + mag->readStart = qmc5883ReadStart; mag->read = qmc5883Read; return true; diff --git a/src/main/drivers/compass/compass_qmc5883p.c b/src/main/drivers/compass/compass_qmc5883p.c index e5c21afb74a..e4a93d4b7e7 100644 --- a/src/main/drivers/compass/compass_qmc5883p.c +++ b/src/main/drivers/compass/compass_qmc5883p.c @@ -123,23 +123,48 @@ static bool qmc5883pInit(magDev_t * mag) return ack; } -static bool qmc5883pRead(magDev_t * mag) +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t qmc5883pStatus; +static uint8_t qmc5883pData[QMC5883P_DATA_BYTES]; + +// Two transfers per sample: the status byte first, the data only when DRDY confirms a fresh sample +static busReadStepResult_e qmc5883pReadStart(magDev_t * mag, bool firstStep) { - uint8_t status; - uint8_t buf[QMC5883P_DATA_BYTES]; + static bool statusStarted = false; - // set magData to zero for case of failed read - mag->magADCRaw[X] = 0; - mag->magADCRaw[Y] = 0; - mag->magADCRaw[Z] = 0; + if (firstStep) { + statusStarted = false; + qmc5883pStatus = 0; + } - bool ack = busRead(mag->busDev, QMC5883P_REG_STATUS, &status); - if (!ack || (status & QMC5883P_STATUS_DRDY_MASK) == 0) { - return false; + if (!statusStarted) { + if (!busReadBufStart(mag->busDev, QMC5883P_REG_STATUS, &qmc5883pStatus, 1)) { + return BUS_READ_STEP_BUSY; + } + statusStarted = true; + return BUS_READ_STEP_NEXT; + } + + if ((qmc5883pStatus & QMC5883P_STATUS_DRDY_MASK) == 0) { + return BUS_READ_STEP_LAST; // nothing new to fetch, qmc5883pRead() reports the miss } - ack = busReadBuf(mag->busDev, QMC5883P_REG_DATA_OUTPUT_X, buf, QMC5883P_DATA_BYTES); - if (!ack) { + if (!busReadBufStart(mag->busDev, QMC5883P_REG_DATA_OUTPUT_X, qmc5883pData, sizeof(qmc5883pData))) { + return BUS_READ_STEP_BUSY; + } + + return BUS_READ_STEP_LAST; +} + +static bool qmc5883pRead(magDev_t * mag) +{ + const uint8_t *buf = qmc5883pData; + + if ((qmc5883pStatus & QMC5883P_STATUS_DRDY_MASK) == 0) { + // set magData to zero for case of failed read + mag->magADCRaw[X] = 0; + mag->magADCRaw[Y] = 0; + mag->magADCRaw[Z] = 0; return false; } @@ -196,6 +221,7 @@ bool qmc5883pDetect(magDev_t * mag) } mag->init = qmc5883pInit; + mag->readStart = qmc5883pReadStart; mag->read = qmc5883pRead; return true; diff --git a/src/main/drivers/compass/compass_rm3100.c b/src/main/drivers/compass/compass_rm3100.c index e4d7ba3afa3..8d31d6340e8 100644 --- a/src/main/drivers/compass/compass_rm3100.c +++ b/src/main/drivers/compass/compass_rm3100.c @@ -96,42 +96,58 @@ static bool deviceInit(magDev_t * mag) return true; } -static bool deviceRead(magDev_t * mag) +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t rm3100Status; +static uint8_t rm3100Report[9]; // X, Y, Z as 24 bit big endian values + +// Two transfers per sample: the status byte first, the data only when a new measurement is ready +static busReadStepResult_e deviceReadStart(magDev_t * mag, bool firstStep) { - uint8_t status; + static bool statusStarted = false; + + if (firstStep) { + statusStarted = false; + rm3100Status = 0; + } + + if (!statusStarted) { + if (!busReadBufStart(mag->busDev, RM3100_REG_STATUS, &rm3100Status, 1)) { + return BUS_READ_STEP_BUSY; + } + statusStarted = true; + return BUS_READ_STEP_NEXT; + } + + if ((rm3100Status & 0x80) == 0) { + return BUS_READ_STEP_LAST; // nothing new to fetch, deviceRead() reports the miss + } + + if (!busReadBufStart(mag->busDev, RM3100_REG_MX, rm3100Report, sizeof(rm3100Report))) { + return BUS_READ_STEP_BUSY; + } + + return BUS_READ_STEP_LAST; +} -#pragma pack(push, 1) - struct { - uint8_t x[3]; - uint8_t y[3]; - uint8_t z[3]; - } rm_report; -#pragma pack(pop) +static bool deviceRead(magDev_t * mag) +{ + const uint8_t *x = &rm3100Report[0]; + const uint8_t *y = &rm3100Report[3]; + const uint8_t *z = &rm3100Report[6]; mag->magADCRaw[X] = 0; mag->magADCRaw[Y] = 0; mag->magADCRaw[Z] = 0; /* Check if new measurement is ready */ - bool ack = busRead(mag->busDev, RM3100_REG_STATUS, &status); - - if (!ack || (status & 0x80) == 0) { + if ((rm3100Status & 0x80) == 0) { return false; } - ack = busReadBuf(mag->busDev, RM3100_REG_MX, (uint8_t *)&rm_report, sizeof(rm_report)); - if (!ack) { - return false; - } - - int32_t xraw; - int32_t yraw; - int32_t zraw; - /* Rearrange mag data */ - xraw = ((rm_report.x[0] << 24) | (rm_report.x[1] << 16) | (rm_report.x[2]) << 8); - yraw = ((rm_report.y[0] << 24) | (rm_report.y[1] << 16) | (rm_report.y[2]) << 8); - zraw = ((rm_report.z[0] << 24) | (rm_report.z[1] << 16) | (rm_report.z[2]) << 8); + const int32_t xraw = ((x[0] << 24) | (x[1] << 16) | (x[2]) << 8); + const int32_t yraw = ((y[0] << 24) | (y[1] << 16) | (y[2]) << 8); + const int32_t zraw = ((z[0] << 24) | (z[1] << 16) | (z[2]) << 8); /* Truncate to 16-bit integers and pass along */ mag->magADCRaw[X] = (int16_t)(xraw >> 16); @@ -171,6 +187,7 @@ bool rm3100MagDetect(magDev_t * mag) } mag->init = deviceInit; + mag->readStart = deviceReadStart; mag->read = deviceRead; return true; diff --git a/src/main/drivers/compass/compass_vcm5883.c b/src/main/drivers/compass/compass_vcm5883.c index 9eb312929bc..6d3b785378f 100644 --- a/src/main/drivers/compass/compass_vcm5883.c +++ b/src/main/drivers/compass/compass_vcm5883.c @@ -78,19 +78,20 @@ static bool vcm5883Init(magDev_t * mag) { return true; } -static bool vcm5883Read(magDev_t * mag) +// Target of the non-blocking data read, filled by the bus driver in the background +static uint8_t vcm5883Data[6]; + +static busReadStepResult_e vcm5883ReadStart(magDev_t * mag, bool firstStep) { - uint8_t buf[6]; + UNUSED(firstStep); - // set magData to zero for case of failed read - mag->magADCRaw[X] = 0; - mag->magADCRaw[Y] = 0; - mag->magADCRaw[Z] = 0; + // Continuous mode, the data registers always hold the latest sample + return busReadBufStart(mag->busDev, VCM5883_REGISTER_ADDR_OUTPUT_X, vcm5883Data, sizeof(vcm5883Data)) ? BUS_READ_STEP_LAST : BUS_READ_STEP_BUSY; +} - bool ack = busReadBuf(mag->busDev, VCM5883_REGISTER_ADDR_OUTPUT_X, buf, 6); - if (!ack) { - return false; - } +static bool vcm5883Read(magDev_t * mag) +{ + const uint8_t *buf = vcm5883Data; mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]); mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]); @@ -112,6 +113,7 @@ bool vcm5883Detect(magDev_t * mag) } mag->init = vcm5883Init; + mag->readStart = vcm5883ReadStart; mag->read = vcm5883Read; return true; diff --git a/src/main/drivers/display_ug2864hsweg01.c b/src/main/drivers/display_ug2864hsweg01.c index 5e808576143..b9dbb88bd6b 100644 --- a/src/main/drivers/display_ug2864hsweg01.c +++ b/src/main/drivers/display_ug2864hsweg01.c @@ -188,83 +188,218 @@ static const uint8_t multiWiiFont[][5] = { // Refer to "Times New Roman" Font Da static busDevice_t *busDev = NULL; -static bool i2c_OLED_send_cmd(uint8_t command) +/* + * The display is drawn into a shadow of its GDDRAM (8 pages of 128 columns). The drawing calls only touch the + * shadow and never wait for the bus; i2c_OLED_pump() streams the columns that changed, one non-blocking transfer + * at a time: a 3 byte page/column address run, then runs of up to OLED_TX_CHUNK_SIZE data bytes. The dashboard + * task calls the pump every millisecond while there is work. A column written while it is on the wire is marked + * dirty again and goes out on the next round, a column rewritten with the same value costs nothing. + */ +#define OLED_PAGE_COUNT (SCREEN_HEIGHT / 8) +#define OLED_TX_CHUNK_SIZE 15 // with the control byte this fits the smallest I2C controller FIFO (RP2350) +#define OLED_CONTROL_COMMANDS 0x00 // Co = 0, D/C = 0: a run of command bytes follows +#define OLED_CONTROL_DATA 0x40 // Co = 0, D/C = 1: a run of display data follows +#define OLED_ERROR_BACKOFF_US 200000 // after a lost chunk leave the bus alone for a while, a dead display must not hog it + +static uint8_t oledFrame[OLED_PAGE_COUNT][SCREEN_WIDTH]; +static uint8_t oledDirtyFirst[OLED_PAGE_COUNT]; // first changed column of the page, SCREEN_WIDTH when the page is clean +static uint8_t oledDirtyLast[OLED_PAGE_COUNT]; // last changed column of the page + +// Drawing cursor, advanced by i2c_OLED_send_byte() +static uint8_t oledCursorPage = 0; +static uint8_t oledCursorCol = 0; + +// The run of dirty columns being streamed and the chunk of it on the wire (the chunk must outlive the transfer) +static bool oledRunActive = false; +static bool oledRunNeedsAddress = true; // the next chunk has to set the page / column address +static uint8_t oledRunPage = 0; +static uint8_t oledRunCol = 0; // next column of the run to send +static uint8_t oledRunEnd = 0; // one past the last column of the run +static uint8_t oledChunk[OLED_TX_CHUNK_SIZE]; +static uint8_t oledChunkLen = 0; // > 0: chunk built, waiting for the bus or in flight +static bool oledChunkIsCmd = false; +static uint8_t oledChunkCol = 0; // first column carried by a data chunk, re-dirtied on a bus error +static bool oledTransferPending = false; +static timeUs_t oledRetryAtUs = 0; // no transfer is started before this time + +// SH1106 has 132-wide GDDRAM but only 128 columns are visible; the first +// 2 columns are hidden, so writes must start 2 columns in. Other controllers +// use the full visible width and need no offset. +static uint8_t oledColumnOffset(void) +{ + return (detectedController == OLED_CONTROLLER_SH1106) ? 2 : 0; +} + +static void oledMarkDirty(uint8_t page, uint8_t first, uint8_t last) +{ + if (first < oledDirtyFirst[page]) { + oledDirtyFirst[page] = first; + } + if (last > oledDirtyLast[page]) { + oledDirtyLast[page] = last; + } +} + +static void oledResetState(void) { + oledTransferPending = false; + oledChunkLen = 0; + oledRunActive = false; + oledRunNeedsAddress = true; + oledRetryAtUs = 0; + oledCursorPage = 0; + oledCursorCol = 0; + for (uint8_t page = 0; page < OLED_PAGE_COUNT; page++) { + oledDirtyFirst[page] = SCREEN_WIDTH; + oledDirtyLast[page] = 0; + } +} + +bool i2c_OLED_pump(void) +{ + bool busError = false; + if (!busDev) { return false; } - return busWrite(busDev, 0x80, command); + if (oledTransferPending) { + if (busIsBusy(busDev, &busError) && !busError) { + return true; // chunk still on the bus + } + oledTransferPending = false; + oledChunkLen = 0; + if (busError) { + // The chunk was lost and the display's column pointer is unknown: redo its columns, address again + if (!oledChunkIsCmd) { + oledMarkDirty(oledRunPage, oledChunkCol, oledRunCol - 1); + } + oledRunNeedsAddress = true; + oledRetryAtUs = micros() + OLED_ERROR_BACKOFF_US; + } + } + + if (oledChunkLen == 0) { + if (cmpTimeUs(micros(), oledRetryAtUs) < 0) { + return false; // backing off after a lost chunk, the dirty columns wait + } + + if (!oledRunActive) { + // Take the dirty columns of the first changed page + uint8_t page = 0; + while (page < OLED_PAGE_COUNT && oledDirtyFirst[page] >= SCREEN_WIDTH) { + page++; + } + if (page >= OLED_PAGE_COUNT) { + return false; + } + oledRunActive = true; + oledRunNeedsAddress = true; + oledRunPage = page; + oledRunCol = oledDirtyFirst[page]; + oledRunEnd = oledDirtyLast[page] + 1; + oledDirtyFirst[page] = SCREEN_WIDTH; // writes from now on mark the page dirty again + oledDirtyLast[page] = 0; + } + + if (oledRunNeedsAddress) { + const uint8_t pixelCol = oledRunCol + oledColumnOffset(); + oledChunk[0] = 0xB0 + oledRunPage; // set page address + oledChunk[1] = 0x00 + (pixelCol & 0x0F); // set low column address + oledChunk[2] = 0x10 + ((pixelCol >> 4) & 0x0F); // set high column address + oledChunkLen = 3; + oledChunkIsCmd = true; + oledRunNeedsAddress = false; + } + else { + const uint8_t remaining = oledRunEnd - oledRunCol; + oledChunkLen = (remaining > OLED_TX_CHUNK_SIZE) ? OLED_TX_CHUNK_SIZE : remaining; + memcpy(oledChunk, &oledFrame[oledRunPage][oledRunCol], oledChunkLen); + oledChunkIsCmd = false; + oledChunkCol = oledRunCol; + oledRunCol += oledChunkLen; + if (oledRunCol >= oledRunEnd) { + oledRunActive = false; // the run is complete once this chunk is out + } + } + } + + // The chunk stays here until the bus takes it + oledTransferPending = busWriteBufStart(busDev, oledChunkIsCmd ? OLED_CONTROL_COMMANDS : OLED_CONTROL_DATA, oledChunk, oledChunkLen); + return true; } -bool i2c_OLED_send_byte(uint8_t val) +bool i2c_OLED_isBusy(void) { if (!busDev) { return false; } + if (oledTransferPending || oledChunkLen > 0 || oledRunActive) { + return true; + } + for (uint8_t page = 0; page < OLED_PAGE_COUNT; page++) { + if (oledDirtyFirst[page] < SCREEN_WIDTH) { + return true; + } + } + return false; +} - return busWrite(busDev, 0x40, val); +uint32_t i2c_OLED_txBytesFree(void) +{ + return sizeof(oledFrame); // drawing goes to the shadow frame and never waits for the bus } -// SH1106 has 132-wide GDDRAM but only 128 columns are visible; the first -// 2 columns are hidden, so writes must start 2 columns in. Other controllers -// use the full visible width and need no offset. -static uint8_t oledColumnOffset(void) +void i2c_OLED_flush(void) { - return (detectedController == OLED_CONTROLLER_SH1106) ? 2 : 0; + while (i2c_OLED_pump()) { + } } -void i2c_OLED_clear_display(void) +bool i2c_OLED_send_byte(uint8_t val) { - // SH1106 only supports page addressing mode; use page-by-page clear for all controllers - uint8_t startCol = oledColumnOffset(); - - i2c_OLED_send_cmd(0xa6); // Set Normal Display - i2c_OLED_send_cmd(0xae); // Display OFF - i2c_OLED_send_cmd(0x40); // Display start line register to 0 - - for (uint8_t page = 0; page < 8; page++) { - i2c_OLED_send_cmd(0xb0 + page); // set page address - i2c_OLED_send_cmd(0x00 + (startCol & 0x0f)); // set low col address - i2c_OLED_send_cmd(0x10 + ((startCol >> 4) & 0x0f)); // set high col address - for (uint8_t col = 0; col < 128; col++) { - i2c_OLED_send_byte(0x00); - } + if (!busDev || oledCursorPage >= OLED_PAGE_COUNT || oledCursorCol >= SCREEN_WIDTH) { + return false; // off the panel, dropped rather than wrapped (the controllers differ here) } - i2c_OLED_send_cmd(0x81); // Setup CONTRAST CONTROL - i2c_OLED_send_cmd(200); // Contrast value (1=dull, 255=very bright) - i2c_OLED_send_cmd(0xaf); // display on + if (oledFrame[oledCursorPage][oledCursorCol] != val) { + oledFrame[oledCursorPage][oledCursorCol] = val; + oledMarkDirty(oledCursorPage, oledCursorCol, oledCursorCol); + } + oledCursorCol++; + return true; } void i2c_OLED_clear_display_quick(void) { - uint8_t startCol = oledColumnOffset(); - - for (uint8_t page = 0; page < 8; page++) { - i2c_OLED_send_cmd(0xb0 + page); // set page address - i2c_OLED_send_cmd(0x00 + (startCol & 0x0f)); // set low col address - i2c_OLED_send_cmd(0x10 + ((startCol >> 4) & 0x0f)); // set high col address - for (uint8_t col = 0; col < 128; col++) { - i2c_OLED_send_byte(0x00); - } + memset(oledFrame, 0, sizeof(oledFrame)); + for (uint8_t page = 0; page < OLED_PAGE_COUNT; page++) { + oledMarkDirty(page, 0, SCREEN_WIDTH - 1); // the panel may hold anything, send the whole frame } } +// Initialisation only, blocking: blanks the panel while it is switched off +void i2c_OLED_clear_display(void) +{ + static const uint8_t displayOff[] = { 0xA6, 0xAE, 0x40 }; // normal display, display off, start line 0 + static const uint8_t displayOn[] = { 0x81, 200, 0xAF }; // contrast (1 = dull, 255 = very bright), display on + + busWriteBuf(busDev, OLED_CONTROL_COMMANDS, displayOff, sizeof(displayOff)); + i2c_OLED_clear_display_quick(); + i2c_OLED_flush(); + busWriteBuf(busDev, OLED_CONTROL_COMMANDS, displayOn, sizeof(displayOn)); +} + void i2c_OLED_set_xy(uint8_t col, uint8_t row) { - uint8_t pixelCol = CHARACTER_WIDTH_TOTAL * col + oledColumnOffset(); - i2c_OLED_send_cmd(0xb0 + row); // set page address - i2c_OLED_send_cmd(0x00 + (pixelCol & 0x0f)); // set low col address - i2c_OLED_send_cmd(0x10 + ((pixelCol >> 4) & 0x0f)); // set high col address + oledCursorPage = row; + oledCursorCol = CHARACTER_WIDTH_TOTAL * col; // the SH1106 column offset is applied when the panel is addressed } void i2c_OLED_set_line(uint8_t row) { - uint8_t startCol = oledColumnOffset(); - i2c_OLED_send_cmd(0xb0 + row); // set page address - i2c_OLED_send_cmd(0x00 + (startCol & 0x0f)); // set low col address - i2c_OLED_send_cmd(0x10 + ((startCol >> 4) & 0x0f)); // set high col address + oledCursorPage = row; + oledCursorCol = 0; } void i2c_OLED_send_char(unsigned char ascii) @@ -358,6 +493,24 @@ static oledControllerType_e detectOledController(void) */ bool ug2864hsweg01InitI2C(void) { + // Sent as one blocking write: the display has to be ready before anything is drawn on it + static const uint8_t initCommands[] = { + 0xD4, 0x80, // Set Display Clock Divide Ratio / OSC Frequency + 0xA8, 0x3F, // Set Multiplex Ratio for 128x64 (64-1) + 0xD3, 0x00, // Set Display Offset + 0x40, // Set Display Start Line + 0x8D, 0x14, // Set Charge Pump (0x10 External, 0x14 Internal DC/DC) + 0xA1, // Set Segment Re-Map + 0xC8, // Set Com Output Scan Direction + 0xDA, 0x12, // Set COM Hardware Configuration + 0x81, 0xCF, // Set Contrast + 0xD9, 0xF1, // Set Pre-Charge Period (0x22 External, 0xF1 Internal) + 0xDB, 0x40, // Set VCOMH Deselect Level + 0xA4, // Set all pixels OFF + 0xA6, // Set display not inverted + 0xAF, // Set display On + }; + busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_UG2864, 0, OWNER_OLED_DISPLAY); if (!busDev) { @@ -365,40 +518,23 @@ bool ug2864hsweg01InitI2C(void) return false; } + oledResetState(); + // Detect the OLED controller type before initialization detectedController = detectOledController(); - // Set display OFF - if (!i2c_OLED_send_cmd(0xAE)) { + // Display OFF, sent directly: its acknowledge tells whether a display is present at all + if (!busWrite(busDev, 0x80, 0xAE)) { LOG_ERROR(SYSTEM, "OLED: Failed to send display OFF command"); return false; } - i2c_OLED_send_cmd(0xD4); // Set Display Clock Divide Ratio / OSC Frequency - i2c_OLED_send_cmd(0x80); // Display Clock Divide Ratio / OSC Frequency - i2c_OLED_send_cmd(0xA8); // Set Multiplex Ratio - i2c_OLED_send_cmd(0x3F); // Multiplex Ratio for 128x64 (64-1) - i2c_OLED_send_cmd(0xD3); // Set Display Offset - i2c_OLED_send_cmd(0x00); // Display Offset - i2c_OLED_send_cmd(0x40); // Set Display Start Line - i2c_OLED_send_cmd(0x8D); // Set Charge Pump - i2c_OLED_send_cmd(0x14); // Charge Pump (0x10 External, 0x14 Internal DC/DC) - - i2c_OLED_send_cmd(0xA1); // Set Segment Re-Map - i2c_OLED_send_cmd(0xC8); // Set Com Output Scan Direction - i2c_OLED_send_cmd(0xDA); // Set COM Hardware Configuration - i2c_OLED_send_cmd(0x12); // COM Hardware Configuration - i2c_OLED_send_cmd(0x81); // Set Contrast - i2c_OLED_send_cmd(0xCF); // Contrast - i2c_OLED_send_cmd(0xD9); // Set Pre-Charge Period - i2c_OLED_send_cmd(0xF1); // Set Pre-Charge Period (0x22 External, 0xF1 Internal) - i2c_OLED_send_cmd(0xDB); // Set VCOMH Deselect Level - i2c_OLED_send_cmd(0x40); // VCOMH Deselect Level - i2c_OLED_send_cmd(0xA4); // Set all pixels OFF - i2c_OLED_send_cmd(0xA6); // Set display not inverted - i2c_OLED_send_cmd(0xAF); // Set display On - - i2c_OLED_clear_display(); + if (!busWriteBuf(busDev, OLED_CONTROL_COMMANDS, initCommands, sizeof(initCommands))) { + LOG_ERROR(SYSTEM, "OLED: Failed to send the init sequence"); + return false; + } + + i2c_OLED_clear_display(); // the panel is blank and switched on when this returns return true; } diff --git a/src/main/drivers/display_ug2864hsweg01.h b/src/main/drivers/display_ug2864hsweg01.h index 45e012b2806..4fcd366a3a5 100644 --- a/src/main/drivers/display_ug2864hsweg01.h +++ b/src/main/drivers/display_ug2864hsweg01.h @@ -41,5 +41,12 @@ void i2c_OLED_set_line(uint8_t row); void i2c_OLED_send_char(unsigned char ascii); void i2c_OLED_send_string(const char *string); bool i2c_OLED_send_byte(uint8_t val); -void i2c_OLED_clear_display(void); +void i2c_OLED_clear_display(void); // blocking, initialisation only void i2c_OLED_clear_display_quick(void); + +// Changed columns are sent in the background, call the pump at this interval while it reports work +#define OLED_PUMP_RETRY_US 1000 +bool i2c_OLED_pump(void); // send the next chunk, true while there is still something to send +bool i2c_OLED_isBusy(void); +uint32_t i2c_OLED_txBytesFree(void); +void i2c_OLED_flush(void); // blocking: send everything that changed diff --git a/src/main/drivers/ina226.c b/src/main/drivers/ina226.c index 00fd23e3f0f..9b4b736c4c1 100644 --- a/src/main/drivers/ina226.c +++ b/src/main/drivers/ina226.c @@ -130,30 +130,68 @@ bool ina226Init(ina226Dev_t *dev, uint8_t i2cBus, uint8_t i2cAddress) return false; } + // Seed the cache so the first readings are available before the background sampling has run + dev->busVoltageValid = ina226ReadRegister(dev, INA226_REG_BUS_VOLTAGE, &dev->rawBusVoltage); + dev->shuntVoltageValid = ina226ReadRegister(dev, INA226_REG_SHUNT_VOLTAGE, &dev->rawShuntVoltage); + dev->pendingReg = 0; + dev->lastReg = INA226_REG_SHUNT_VOLTAGE; + return true; } -bool ina226ReadBusVoltage(ina226Dev_t *dev, uint16_t *centiVolts) +void ina226Update(ina226Dev_t *dev) { - uint16_t rawBusVoltage; + if (!dev || !dev->busDev) { + return; + } - if (!centiVolts || !ina226ReadRegister(dev, INA226_REG_BUS_VOLTAGE, &rawBusVoltage)) { + bool busError = false; + + if (dev->pendingReg != 0) { + if (busIsBusy(dev->busDev, &busError) && !busError) { + return; // still on the bus, look again on the next call + } + + const uint16_t value = ((uint16_t)dev->rxBuf[0] << 8) | dev->rxBuf[1]; + if (dev->pendingReg == INA226_REG_BUS_VOLTAGE) { + dev->busVoltageValid = !busError; + if (!busError) { + dev->rawBusVoltage = value; + } + } else { + dev->shuntVoltageValid = !busError; + if (!busError) { + dev->rawShuntVoltage = value; + } + } + dev->lastReg = dev->pendingReg; + dev->pendingReg = 0; + } + + // Alternate between the two registers, one transfer per call + const uint8_t reg = (dev->lastReg == INA226_REG_BUS_VOLTAGE) ? INA226_REG_SHUNT_VOLTAGE : INA226_REG_BUS_VOLTAGE; + if (busReadBufStart(dev->busDev, reg, dev->rxBuf, sizeof(dev->rxBuf))) { + dev->pendingReg = reg; + } +} + +bool ina226ReadBusVoltage(ina226Dev_t *dev, uint16_t *centiVolts) +{ + if (!dev || !centiVolts || !dev->busVoltageValid) { return false; } - *centiVolts = ina226BusVoltageToCentivolts(rawBusVoltage); + *centiVolts = ina226BusVoltageToCentivolts(dev->rawBusVoltage); return true; } bool ina226ReadShuntCurrent(ina226Dev_t *dev, uint32_t shuntMicroOhm, int16_t *centiAmps) { - uint16_t rawShuntVoltage; - - if (!centiAmps || !ina226ReadRegister(dev, INA226_REG_SHUNT_VOLTAGE, &rawShuntVoltage)) { + if (!dev || !centiAmps || !dev->shuntVoltageValid) { return false; } - *centiAmps = ina226ShuntVoltageToCentiamps((int16_t)rawShuntVoltage, shuntMicroOhm); + *centiAmps = ina226ShuntVoltageToCentiamps((int16_t)dev->rawShuntVoltage, shuntMicroOhm); return true; } diff --git a/src/main/drivers/ina226.h b/src/main/drivers/ina226.h index f3f89786234..c7e8adbfd68 100644 --- a/src/main/drivers/ina226.h +++ b/src/main/drivers/ina226.h @@ -22,10 +22,20 @@ typedef struct ina226Dev_s { busDevice_t *busDev; + + // Background sampling driven by ina226Update(), the read functions return the last completed values + uint8_t rxBuf[2]; + uint8_t pendingReg; // register being read, 0 = none + uint8_t lastReg; // register read last, the two are alternated + uint16_t rawBusVoltage; + uint16_t rawShuntVoltage; + bool busVoltageValid; + bool shuntVoltageValid; } ina226Dev_t; bool ina226Init(ina226Dev_t *dev, uint8_t i2cBus, uint8_t i2cAddress); bool ina226Detect(ina226Dev_t *dev); +void ina226Update(ina226Dev_t *dev); // call periodically, alternates non-blocking reads of bus and shunt voltage bool ina226ReadBusVoltage(ina226Dev_t *dev, uint16_t *centiVolts); bool ina226ReadShuntCurrent(ina226Dev_t *dev, uint32_t shuntMicroOhm, int16_t *centiAmps); diff --git a/src/main/drivers/io_pcf8574.c b/src/main/drivers/io_pcf8574.c index b42386cf9b3..e3ec7558128 100644 --- a/src/main/drivers/io_pcf8574.c +++ b/src/main/drivers/io_pcf8574.c @@ -73,7 +73,10 @@ bool pcf8574Init(void) void pcf8574Write(uint8_t data) { - busWrite(busDev, PCF8574_WRITE_ADDRESS, data); + // Fire and forget; the blocking write is the fallback when another device's transfer is in flight + if (!busWriteStart(busDev, PCF8574_WRITE_ADDRESS, data)) { + busWrite(busDev, PCF8574_WRITE_ADDRESS, data); + } } uint8_t pcf8574Read(void) diff --git a/src/main/drivers/irlock.c b/src/main/drivers/irlock.c index 2ce13213cb7..2e9e53f2f6c 100644 --- a/src/main/drivers/irlock.c +++ b/src/main/drivers/irlock.c @@ -34,26 +34,62 @@ static bool irlockHealthy = false; -static bool irlockFrameSync(irlockDev_t *irlockDev) -{ - uint32_t sync_word = 0; - uint8_t count = 10; - while (count-- && sync_word != IRLOCK_FRAME_SYNC) { - uint8_t sync_byte; - irlockHealthy = busRead(irlockDev->busDev, 0xFF, &sync_byte); - if (!(irlockHealthy && sync_byte)) return false; - sync_word = (sync_word >> 8) | (((uint32_t)sync_byte) << 24); - } - return sync_word == IRLOCK_FRAME_SYNC; -} +// The sensor streams frames; the blocking driver scanned up to this many bytes for the frame sync per call +#define IRLOCK_SYNC_READ_LIMIT 10 -static bool irlockRead(irlockDev_t *irlockDev, irlockData_t *irlockData) +static irlockReadResult_e irlockRead(irlockDev_t *irlockDev, irlockData_t *irlockData) { - if (irlockFrameSync(irlockDev) && busReadBuf(irlockDev->busDev, 0xFF, (void*)irlockData, sizeof(*irlockData))) { - uint16_t cksum = irlockData->signature + irlockData->posX + irlockData->posY + irlockData->sizeX + irlockData->sizeY; - if (irlockData->cksum == cksum) return true; + static uint32_t syncWord = 0; + static uint8_t syncReads = 0; + static uint8_t syncByte; // target of the one byte sync reads, filled in the background + static bool transferPending = false; + static bool frameRequested = false; // the pending transfer is the frame itself + bool busError = false; + + if (transferPending) { + if (busIsBusy(irlockDev->busDev, &busError) && !busError) { + return IRLOCK_READ_PENDING; + } + transferPending = false; + irlockHealthy = !busError; + + if (busError) { + syncWord = 0; + syncReads = 0; + frameRequested = false; + return IRLOCK_READ_NO_DATA; + } + + if (frameRequested) { + frameRequested = false; + syncWord = 0; + syncReads = 0; + const uint16_t cksum = irlockData->signature + irlockData->posX + irlockData->posY + irlockData->sizeX + irlockData->sizeY; + return (irlockData->cksum == cksum) ? IRLOCK_READ_FRAME : IRLOCK_READ_NO_DATA; + } + + // one more byte of the sync scan arrived + if (syncByte == 0) { + syncWord = 0; // nothing streaming, give up until the next regular call + syncReads = 0; + return IRLOCK_READ_NO_DATA; + } + syncWord = (syncWord >> 8) | (((uint32_t)syncByte) << 24); + if (syncWord == IRLOCK_FRAME_SYNC) { + transferPending = busReadBufStart(irlockDev->busDev, 0xFF, (uint8_t *)irlockData, sizeof(*irlockData)); + frameRequested = transferPending; + return IRLOCK_READ_PENDING; + } + if (++syncReads >= IRLOCK_SYNC_READ_LIMIT) { + syncWord = 0; + syncReads = 0; + return IRLOCK_READ_NO_DATA; + } } - return false; + + // scan for the frame sync one byte at a time + transferPending = busReadBufStart(irlockDev->busDev, 0xFF, &syncByte, 1); + return IRLOCK_READ_PENDING; } static bool deviceDetect(irlockDev_t *irlockDev) diff --git a/src/main/drivers/irlock.h b/src/main/drivers/irlock.h index 55e01475c93..92d2d9901e3 100644 --- a/src/main/drivers/irlock.h +++ b/src/main/drivers/irlock.h @@ -34,9 +34,15 @@ typedef struct { uint16_t sizeY; } irlockData_t; +typedef enum { + IRLOCK_READ_NO_DATA = 0, // no complete frame this time, try again on the regular period + IRLOCK_READ_PENDING, // a transfer is on the bus, call again shortly + IRLOCK_READ_FRAME, // irlockData holds a fresh, checksum verified frame +} irlockReadResult_e; + typedef struct irlockDev_s { busDevice_t *busDev; - bool (*read)(struct irlockDev_s *irlockDev, irlockData_t *irlockData); + irlockReadResult_e (*read)(struct irlockDev_s *irlockDev, irlockData_t *irlockData); // non-blocking, see irlockReadResult_e } irlockDev_t; bool irlockDetect(irlockDev_t *irlockDev); diff --git a/src/main/drivers/pitotmeter/pitotmeter.h b/src/main/drivers/pitotmeter/pitotmeter.h index 562a87d8cd0..b7475b99756 100644 --- a/src/main/drivers/pitotmeter/pitotmeter.h +++ b/src/main/drivers/pitotmeter/pitotmeter.h @@ -21,13 +21,20 @@ struct pitotDev_s; typedef bool (*pitotOpFuncPtr)(struct pitotDev_s * pitot); // pitot start operation +typedef busReadStepResult_e (*pitotReadStartFuncPtr)(struct pitotDev_s * pitot, bool firstStep); // non-blocking read step, see drivers/bus.h typedef void (*pitotCalculateFuncPtr)(struct pitotDev_s * pitot, float *pressure, float *temperature); // airspeed calculation (filled params are pressure and temperature) +/* + * start() triggers a measurement (may be a non-blocking bus transfer), get() delivers it after `delay`. + * A driver with readStart set fetches the data over one or more non-blocking transfers; get() then only parses them. + * Without readStart, get() does its own blocking bus access. + */ typedef struct pitotDev_s { busDevice_t * busDev; uint16_t delay; float calibThreshold; pitotOpFuncPtr start; + pitotReadStartFuncPtr readStart; pitotOpFuncPtr get; pitotCalculateFuncPtr calculate; } pitotDev_t; diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c index d899ac05a9e..97f01af6949 100755 --- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c +++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c @@ -67,17 +67,21 @@ static bool dlvr_start(pitotDev_t * pitot) return true; } -static bool dlvr_read(pitotDev_t * pitot) +// Target of the non-blocking data read, filled by the bus driver in the background +static uint8_t dlvrFrame[4]; + +static busReadStepResult_e dlvr_read_start(pitotDev_t * pitot, bool firstStep) { - uint8_t rxbuf1[4]; + UNUSED(firstStep); + return busReadBufStart(pitot->busDev, 0xFF, dlvrFrame, sizeof(dlvrFrame)) ? BUS_READ_STEP_LAST : BUS_READ_STEP_BUSY; +} +static bool dlvr_read(pitotDev_t * pitot) +{ + const uint8_t *rxbuf1 = dlvrFrame; dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); ctx->dataValid = false; - if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) { - return false; - } - // status = 00 -> ok, new data // status = 01 -> reserved // status = 10 -> ok, data stale @@ -159,6 +163,7 @@ bool dlvrDetect(pitotDev_t * pitot) pitot->delay = 10000; pitot->calibThreshold = 0.00001f; // low noise sensor pitot->start = dlvr_start; + pitot->readStart = dlvr_read_start; pitot->get = dlvr_read; pitot->calculate = dlvr_calculate; return true; diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c index c5272ccb217..89d844488f6 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c +++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c @@ -38,29 +38,41 @@ typedef struct __attribute__ ((__packed__)) ms4525Ctx_s { STATIC_ASSERT(sizeof(ms4525Ctx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small); +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t ms4525Frames[2][4]; // two consecutive data frames, compared against each other +static uint8_t ms4525MeasurementRequest; // byte returned by the measurement request command + static bool ms4525_start(pitotDev_t * pitot) { - uint8_t rxbuf[1]; - bool ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 1); - return ack; + // A one byte read is the measurement request command, the answer itself is not used + return busReadBufStart(pitot->busDev, 0xFF, &ms4525MeasurementRequest, 1); } -static bool ms4525_read(pitotDev_t * pitot) +// Two data frames per sample, read back to back +static busReadStepResult_e ms4525_read_start(pitotDev_t * pitot, bool firstStep) { - uint8_t rxbuf1[4]; - uint8_t rxbuf2[4]; - - ms4525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); - ctx->dataValid = false; + static uint8_t frame = 0; - if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) { - return false; + if (firstStep) { + frame = 0; } - if (!busReadBuf(pitot->busDev, 0xFF, rxbuf2, 4)) { - return false; + if (!busReadBufStart(pitot->busDev, 0xFF, ms4525Frames[frame], sizeof(ms4525Frames[frame]))) { + return BUS_READ_STEP_BUSY; } + frame++; + return (frame < 2) ? BUS_READ_STEP_NEXT : BUS_READ_STEP_LAST; +} + +static bool ms4525_read(pitotDev_t * pitot) +{ + const uint8_t *rxbuf1 = ms4525Frames[0]; + const uint8_t *rxbuf2 = ms4525Frames[1]; + + ms4525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + ctx->dataValid = false; + const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6); if (status == 2 || status == 3) { return false; @@ -146,6 +158,7 @@ bool ms4525Detect(pitotDev_t * pitot) pitot->delay = 10000; pitot->calibThreshold = 0.00005f; // noisy sensor pitot->start = ms4525_start; + pitot->readStart = ms4525_read_start; pitot->get = ms4525_read; pitot->calculate = ms4525_calculate; return true; diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms5525.c b/src/main/drivers/pitotmeter/pitotmeter_ms5525.c index 303c4becb5f..0b0a5bce446 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_ms5525.c +++ b/src/main/drivers/pitotmeter/pitotmeter_ms5525.c @@ -79,14 +79,18 @@ static int8_t ms5525_crc(uint16_t *prom) return -1; } -static bool ms5525_read_adc(pitotDev_t *pitot, uint32_t *result) +// Target of the non-blocking ADC read, filled by the bus driver in the background +static uint8_t ms5525AdcBuf[3]; + +static busReadStepResult_e ms5525_read_start(pitotDev_t * pitot, bool firstStep) { - uint8_t rxbuf[3]; - if (busReadBuf(pitot->busDev, CMD_ADC_READ, rxbuf, 3)) { - *result = (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2]; - return true; - } - return false; + UNUSED(firstStep); + return busReadBufStart(pitot->busDev, CMD_ADC_READ, ms5525AdcBuf, sizeof(ms5525AdcBuf)) ? BUS_READ_STEP_LAST : BUS_READ_STEP_BUSY; +} + +static uint32_t ms5525_adc_value(void) +{ + return (ms5525AdcBuf[0] << 16) | (ms5525AdcBuf[1] << 8) | ms5525AdcBuf[2]; } static bool ms5525_start(pitotDev_t * pitot) @@ -95,9 +99,9 @@ static bool ms5525_start(pitotDev_t * pitot) uint8_t step = ctx->up >> 24; if (step == 0) { - return busWrite(pitot->busDev, CMD_ADC_CONV + CMD_ADC_D1 + CMD_ADC_4096, 1); + return busWriteStart(pitot->busDev, CMD_ADC_CONV + CMD_ADC_D1 + CMD_ADC_4096, 1); } else { - return busWrite(pitot->busDev, CMD_ADC_CONV + CMD_ADC_D2 + CMD_ADC_4096, 1); + return busWriteStart(pitot->busDev, CMD_ADC_CONV + CMD_ADC_D2 + CMD_ADC_4096, 1); } } @@ -105,10 +109,10 @@ static bool ms5525_read(pitotDev_t * pitot) { ms5525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); uint8_t step = ctx->up >> 24; - - uint32_t adc_val = 0; - if (!ms5525_read_adc(pitot, &adc_val)) { - return false; + const uint32_t adc_val = ms5525_adc_value(); + + if (adc_val == 0) { + return false; // ADC read before the conversion finished } if (step == 0) { @@ -219,6 +223,7 @@ bool ms5525Detect(pitotDev_t * pitot) pitot->delay = 10000; // max 9.04ms for OSR4096 pitot->calibThreshold = 0.00005f; pitot->start = ms5525_start; + pitot->readStart = ms5525_read_start; pitot->get = ms5525_read; pitot->calculate = ms5525_calculate; diff --git a/src/main/drivers/rangefinder/rangefinder.h b/src/main/drivers/rangefinder/rangefinder.h index 797a29a6c03..ed268cfc22e 100644 --- a/src/main/drivers/rangefinder/rangefinder.h +++ b/src/main/drivers/rangefinder/rangefinder.h @@ -27,8 +27,12 @@ struct rangefinderDev_s; +// Delay before a driver looks at a pending non-blocking bus transfer again +#define RANGEFINDER_UPDATE_RETRY_US 1000 + typedef void (*rangefinderOpInitFuncPtr)(struct rangefinderDev_s * dev); -typedef void (*rangefinderOpStartFuncPtr)(struct rangefinderDev_s * dev); +// Returns the delay until the next update() call in microseconds, 0 = the regular delayMs period +typedef timeDelta_t (*rangefinderOpStartFuncPtr)(struct rangefinderDev_s * dev); typedef int32_t (*rangefinderOpReadFuncPtr)(struct rangefinderDev_s * dev); typedef struct rangefinderDev_s { diff --git a/src/main/drivers/rangefinder/rangefinder_srf10.c b/src/main/drivers/rangefinder/rangefinder_srf10.c index e5bdf89f17e..22b805e3ff0 100644 --- a/src/main/drivers/rangefinder/rangefinder_srf10.c +++ b/src/main/drivers/rangefinder/rangefinder_srf10.c @@ -104,33 +104,76 @@ static void srf10_init(rangefinderDev_t * rangefinder) * Start a range reading * Called periodically by the scheduler */ -static void srf10_start_reading(rangefinderDev_t * rangefinder) +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t srf10Revision; +static uint8_t srf10RangeHigh; +static uint8_t srf10RangeLow; + +typedef enum { + SRF10_STEP_REVISION = 0, // the revision register reads 0xFF while a ranging is in progress + SRF10_STEP_RANGE_HIGH, + SRF10_STEP_RANGE_LOW, + SRF10_STEP_FIRE, // start the next ranging +} srf10Step_e; + +static timeDelta_t srf10_start_reading(rangefinderDev_t * rangefinder) { - uint8_t revision; + static srf10Step_e step = SRF10_STEP_REVISION; + static bool transferPending = false; + bool busError = false; - // check if there is a measurement outstanding, 0xFF is returned if no measurement - isSensorResponding = busRead(rangefinder->busDev, SRF10_READ_SoftwareRevision, &revision); - - if (isSensorResponding && revision != 0xFF) { - // there is a measurement - uint8_t lowByte, highByte; - - isSensorResponding = busRead(rangefinder->busDev, SRF10_READ_RangeLowByte, &lowByte); - isSensorResponding = busRead(rangefinder->busDev, SRF10_READ_RangeHighByte, &highByte); + if (transferPending) { + if (busIsBusy(rangefinder->busDev, &busError) && !busError) { + return RANGEFINDER_UPDATE_RETRY_US; + } + transferPending = false; + isSensorResponding = !busError; - srf10measurementCm = highByte << 8 | lowByte; + if (busError) { + step = SRF10_STEP_REVISION; + return 0; + } - if (srf10measurementCm > SRF10_MAX_RANGE_CM) { - srf10measurementCm = RANGEFINDER_OUT_OF_RANGE; + switch (step) { + case SRF10_STEP_REVISION: + step = (srf10Revision != 0xFF) ? SRF10_STEP_RANGE_HIGH : SRF10_STEP_FIRE; + break; + case SRF10_STEP_RANGE_HIGH: + step = SRF10_STEP_RANGE_LOW; + break; + case SRF10_STEP_RANGE_LOW: + srf10measurementCm = srf10RangeHigh << 8 | srf10RangeLow; + if (srf10measurementCm > SRF10_MAX_RANGE_CM) { + srf10measurementCm = RANGEFINDER_OUT_OF_RANGE; + } + step = SRF10_STEP_FIRE; + break; + default: + step = SRF10_STEP_REVISION; + return 0; // cycle complete, next one after the regular period } } - const timeMs_t timeNowMs = millis(); - if (timeNowMs > timeOfLastMeasurementMs + minimumFiringIntervalMs) { - // measurement repeat interval should be greater than minimumFiringIntervalMs - // to avoid interference between connective measurements. - timeOfLastMeasurementMs = timeNowMs; - busWrite(rangefinder->busDev, SRF10_WRITE_CommandRegister, SRF10_COMMAND_InitiateRangingCm); + switch (step) { + case SRF10_STEP_REVISION: + transferPending = busReadBufStart(rangefinder->busDev, SRF10_READ_SoftwareRevision, &srf10Revision, 1); + return RANGEFINDER_UPDATE_RETRY_US; + case SRF10_STEP_RANGE_HIGH: + transferPending = busReadBufStart(rangefinder->busDev, SRF10_READ_RangeHighByte, &srf10RangeHigh, 1); + return RANGEFINDER_UPDATE_RETRY_US; + case SRF10_STEP_RANGE_LOW: + transferPending = busReadBufStart(rangefinder->busDev, SRF10_READ_RangeLowByte, &srf10RangeLow, 1); + return RANGEFINDER_UPDATE_RETRY_US; + default: { + const timeMs_t timeNowMs = millis(); + if (timeNowMs > timeOfLastMeasurementMs + minimumFiringIntervalMs) { + timeOfLastMeasurementMs = timeNowMs; + transferPending = busWriteStart(rangefinder->busDev, SRF10_WRITE_CommandRegister, SRF10_COMMAND_InitiateRangingCm); + return RANGEFINDER_UPDATE_RETRY_US; + } + step = SRF10_STEP_REVISION; + return 0; + } } } diff --git a/src/main/drivers/rangefinder/rangefinder_teraranger_evo.c b/src/main/drivers/rangefinder/rangefinder_teraranger_evo.c index 4bc10463443..7b9827d31a3 100644 --- a/src/main/drivers/rangefinder/rangefinder_teraranger_evo.c +++ b/src/main/drivers/rangefinder/rangefinder_teraranger_evo.c @@ -71,6 +71,11 @@ static void triggerNewReading(rangefinderDev_t *rangefinder){ busWrite(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, 0x00); //request to next measure, scheduler is much slower than 500uS to we need to wait between write and read } +typedef enum { + TERA_STEP_READ = 0, // fetch the last measurement + TERA_STEP_TRIGGER, // request the next one +} teraStep_e; + static void teraRangerInit(rangefinderDev_t *rangefinder){ triggerNewReading(rangefinder); } @@ -94,27 +99,50 @@ static bool checkCrc(void){ return teraRangerEvo.dataBuff[2] == crc8_update(0, teraRangerEvo.dataBuff, 2); } -void teraRangerUpdate(rangefinderDev_t *rangefinder){ - if (busReadBuf(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, teraRangerEvo.dataBuff, 3)) { - if (!checkCrc()) { - teraRangerEvo.teraRangerMeasurementCm = RANGEFINDER_NO_NEW_DATA; - triggerNewReading(rangefinder); - return; +timeDelta_t teraRangerUpdate(rangefinderDev_t *rangefinder){ + static teraStep_e step = TERA_STEP_READ; + static bool transferPending = false; + bool busError = false; + + if (transferPending) { + if (busIsBusy(rangefinder->busDev, &busError) && !busError) { + return RANGEFINDER_UPDATE_RETRY_US; + } + transferPending = false; + + if (step == TERA_STEP_TRIGGER) { + step = TERA_STEP_READ; + return 0; // cycle complete, next one after the regular period } - const int32_t teraRangerMeasurementMM = ((int32_t)teraRangerEvo.dataBuff[0] << 8 | (int32_t)teraRangerEvo.dataBuff[1]); - if (teraRangerMeasurementMM == TERARANGER_EVO_VALUE_TOO_CLOSE || teraRangerMeasurementMM == TERARANGER_EVO_VALUE_OUT_OF_RANGE) { - teraRangerEvo.teraRangerMeasurementCm = RANGEFINDER_OUT_OF_RANGE; - triggerNewReading(rangefinder); - return; + // the measurement is in teraRangerEvo.dataBuff + if (busError) { + teraRangerEvo.teraRangerMeasurementCm = RANGEFINDER_HARDWARE_FAILURE; + } else if (!checkCrc()) { + teraRangerEvo.teraRangerMeasurementCm = RANGEFINDER_NO_NEW_DATA; + } else { + const int32_t teraRangerMeasurementMM = ((int32_t)teraRangerEvo.dataBuff[0] << 8 | (int32_t)teraRangerEvo.dataBuff[1]); + if (teraRangerMeasurementMM == TERARANGER_EVO_VALUE_TOO_CLOSE || teraRangerMeasurementMM == TERARANGER_EVO_VALUE_OUT_OF_RANGE) { + teraRangerEvo.teraRangerMeasurementCm = RANGEFINDER_OUT_OF_RANGE; + } else { + teraRangerEvo.teraRangerMeasurementCm = MILLIMETERS_TO_CENTIMETERS(teraRangerMeasurementMM); + } } + step = TERA_STEP_TRIGGER; + } - teraRangerEvo.teraRangerMeasurementCm = MILLIMETERS_TO_CENTIMETERS(teraRangerMeasurementMM); - } else { - teraRangerEvo.teraRangerMeasurementCm = RANGEFINDER_HARDWARE_FAILURE; + if (step == TERA_STEP_READ) { + if (busReadBufStart(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, teraRangerEvo.dataBuff, sizeof(teraRangerEvo.dataBuff))) { + transferPending = true; + } + return RANGEFINDER_UPDATE_RETRY_US; } - triggerNewReading(rangefinder); + // request the next measurement, it is read on the next regular cycle + if (busWriteStart(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, 0x00)) { + transferPending = true; + } + return RANGEFINDER_UPDATE_RETRY_US; } diff --git a/src/main/drivers/rangefinder/rangefinder_tof10120_i2c.c b/src/main/drivers/rangefinder/rangefinder_tof10120_i2c.c index 5c47cefe7f0..56ca89424e9 100644 --- a/src/main/drivers/rangefinder/rangefinder_tof10120_i2c.c +++ b/src/main/drivers/rangefinder/rangefinder_tof10120_i2c.c @@ -49,25 +49,40 @@ static void tof10120i2cInit(rangefinderDev_t *rangefinder) delay(100); } -void tof10120i2cUpdate(rangefinderDev_t *rangefinder) +// Target of the non-blocking read, filled by the bus driver in the background +static uint8_t tof10120Frame[2]; +static bool tof10120TransferPending = false; + +timeDelta_t tof10120i2cUpdate(rangefinderDev_t *rangefinder) { - uint8_t buffer[2]; - uint16_t distance_mm; + bool busError = false; + + if (!tof10120TransferPending) { + if (busReadBufStart(rangefinder->busDev, TOF10120_I2C_REGISTRY_RT_DISTANCE, tof10120Frame, sizeof(tof10120Frame))) { + tof10120TransferPending = true; + } + return RANGEFINDER_UPDATE_RETRY_US; // wait for the transfer, or for the bus to free up + } - isTof10120Responding = busReadBuf(rangefinder->busDev, TOF10120_I2C_REGISTRY_RT_DISTANCE, buffer, sizeof(buffer)); + if (busIsBusy(rangefinder->busDev, &busError) && !busError) { + return RANGEFINDER_UPDATE_RETRY_US; + } + tof10120TransferPending = false; + isTof10120Responding = !busError; if (!isTof10120Responding) { - return; + return 0; } - distance_mm = (buffer[0] << 8) | buffer[1]; + const uint16_t distance_mm = (tof10120Frame[0] << 8) | tof10120Frame[1]; if (distance_mm >= TOF10120_I2C_MAX_RANGE_MM) { tof10120MeasurementCm = RANGEFINDER_OUT_OF_RANGE; - return; + return 0; } tof10120MeasurementCm = (int32_t)roundf(distance_mm / 10); + return 0; } /** diff --git a/src/main/drivers/rangefinder/rangefinder_us42.c b/src/main/drivers/rangefinder/rangefinder_us42.c index eabb571d927..5617b952257 100644 --- a/src/main/drivers/rangefinder/rangefinder_us42.c +++ b/src/main/drivers/rangefinder/rangefinder_us42.c @@ -56,29 +56,63 @@ static void us42Init(rangefinderDev_t *rangefinder) busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0); } -void us42Update(rangefinderDev_t *rangefinder) -{ - uint8_t data[2]; - isUs42Responding = busReadBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, data, 2); +// Target of the non-blocking read, filled by the bus driver in the background +static uint8_t us42Frame[2]; - if (isUs42Responding) { - us42MeasurementCm = (int32_t)data[0] << 8 | (int32_t)data[1]; +typedef enum { + US42_STEP_READ = 0, // fetch the last measurement + US42_STEP_PROBE, // ask for the next one +} us42Step_e; - if (us42MeasurementCm > US42_MAX_RANGE_CM) { - us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE; - } +timeDelta_t us42Update(rangefinderDev_t *rangefinder) +{ + static us42Step_e step = US42_STEP_READ; + static bool transferPending = false; + bool busError = false; - } else { - us42MeasurementCm = RANGEFINDER_HARDWARE_FAILURE; - } + if (transferPending) { + if (busIsBusy(rangefinder->busDev, &busError) && !busError) { + return RANGEFINDER_UPDATE_RETRY_US; + } + transferPending = false; + + if (step == US42_STEP_READ) { + isUs42Responding = !busError; + if (isUs42Responding) { + us42MeasurementCm = (int32_t)us42Frame[0] << 8 | (int32_t)us42Frame[1]; + if (us42MeasurementCm > US42_MAX_RANGE_CM) { + us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE; + } + } else { + us42MeasurementCm = RANGEFINDER_HARDWARE_FAILURE; + } + step = US42_STEP_PROBE; + } + else { + step = US42_STEP_READ; + return 0; // cycle complete, next one after the regular period + } + } + + if (step == US42_STEP_READ) { + if (busReadBufStart(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, us42Frame, sizeof(us42Frame))) { + transferPending = true; + } + return RANGEFINDER_UPDATE_RETRY_US; + } const timeMs_t timeNowMs = millis(); if (timeNowMs > timeOfLastMeasurementMs + minimumReadingIntervalMs) { - // measurement repeat interval should be greater than minimumReadingIntervalMs - // to avoid interference between connective measurements. timeOfLastMeasurementMs = timeNowMs; - busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0); + // The probe command is the register byte alone + if (busWriteBufStart(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0)) { + transferPending = true; + } + return RANGEFINDER_UPDATE_RETRY_US; } + + step = US42_STEP_READ; + return 0; } /** diff --git a/src/main/drivers/rangefinder/rangefinder_virtual.c b/src/main/drivers/rangefinder/rangefinder_virtual.c index c3b9bab6ec5..30030f5b6ed 100644 --- a/src/main/drivers/rangefinder/rangefinder_virtual.c +++ b/src/main/drivers/rangefinder/rangefinder_virtual.c @@ -46,10 +46,11 @@ static void virtualRangefinderInit(rangefinderDev_t * dev) return highLevelDeviceVTable->init(); } -static void virtualRangefinderUpdate(rangefinderDev_t * dev) +static timeDelta_t virtualRangefinderUpdate(rangefinderDev_t * dev) { UNUSED(dev); - return highLevelDeviceVTable->update(); + highLevelDeviceVTable->update(); + return 0; } static int32_t virtualRangefinderGetDistance(rangefinderDev_t * dev) diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l0x.c b/src/main/drivers/rangefinder/rangefinder_vl53l0x.c index d2ddea34139..9194833db89 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l0x.c +++ b/src/main/drivers/rangefinder/rangefinder_vl53l0x.c @@ -220,9 +220,11 @@ typedef enum { } vcselPeriodType_e; typedef enum { - MEASUREMENT_START, - MEASUREMENT_WAIT, - MEASUREMENT_READ, + MEASUREMENT_START, // write the start sequence, one register per call + MEASUREMENT_WAIT, // wait for SYSRANGE_START to clear + MEASUREMENT_READ_STATUS, // wait for the interrupt status to report a result + MEASUREMENT_READ_RESULT, // fetch the range + MEASUREMENT_CLEAR, // clear the interrupt } measurementSteps_e; typedef struct { @@ -1019,113 +1021,134 @@ static void vl53l0x_Init(rangefinderDev_t * rangefinder) isInitialized = true; } -void vl53l0x_Update(rangefinderDev_t * rangefinder) +// Register writes that start a single measurement, issued one per call. Entry 3 carries the stop variable. +static const struct { + uint8_t reg; + uint8_t value; +} vl53l0xStartSequence[] = { + { 0x80, 0x01 }, + { 0xFF, 0x01 }, + { 0x00, 0x00 }, + { 0x91, 0x00 }, // stopVariable + { 0x00, 0x01 }, + { 0xFF, 0x00 }, + { 0x80, 0x00 }, + { VL53L0X_REG_SYSRANGE_START, 0x01 }, +}; +#define VL53L0X_START_SEQUENCE_STOP_VARIABLE_INDEX 3 + +// Targets of the non-blocking reads, filled by the bus driver in the background +static uint8_t vl53l0xRegByte; +static uint8_t vl53l0xResult[2]; + +timeDelta_t vl53l0x_Update(rangefinderDev_t * rangefinder) { + static bool transferPending = false; // a non-blocking transfer of the current step is on the bus + static bool stepIssued = false; // the current step started its transfer, the next call evaluates it + static uint8_t startIndex = 0; + busDevice_t * busDev = rangefinder->busDev; + bool busError = false; + if (!isInitialized) { - return; + return 0; } -#if 1 // SINGLE MEASUREMENT MODE + if (transferPending) { + if (busIsBusy(busDev, &busError) && !busError) { + return RANGEFINDER_UPDATE_RETRY_US; + } + transferPending = false; + isResponding = !busError; + if (busError) { + measSteps = MEASUREMENT_START; + startIndex = 0; + stepIssued = false; + return 0; + } + } + + // SINGLE MEASUREMENT MODE switch (measSteps) { case MEASUREMENT_START: - // Initiate new measurement - writeReg(rangefinder->busDev, 0x80, 0x01); - writeReg(rangefinder->busDev, 0xFF, 0x01); - writeReg(rangefinder->busDev, 0x00, 0x00); - writeReg(rangefinder->busDev, 0x91, stopVariable); - writeReg(rangefinder->busDev, 0x00, 0x01); - writeReg(rangefinder->busDev, 0xFF, 0x00); - writeReg(rangefinder->busDev, 0x80, 0x00); - writeReg(rangefinder->busDev, VL53L0X_REG_SYSRANGE_START, 0x01); - + // Initiate new measurement, one register write per call + if (startIndex < ARRAYLEN(vl53l0xStartSequence)) { + const uint8_t value = (startIndex == VL53L0X_START_SEQUENCE_STOP_VARIABLE_INDEX) ? stopVariable : vl53l0xStartSequence[startIndex].value; + if (busWriteStart(busDev, vl53l0xStartSequence[startIndex].reg, value)) { + startIndex++; + transferPending = true; + } + return RANGEFINDER_UPDATE_RETRY_US; + } + startIndex = 0; startTimeout(); measSteps = MEASUREMENT_WAIT; - break; + FALLTHROUGH; case MEASUREMENT_WAIT: - // Wait for data and read the measurement - if (readReg(rangefinder->busDev, VL53L0X_REG_SYSRANGE_START) & 0x01) { + // Wait for the measurement to start + if (!stepIssued) { + transferPending = busReadBufStart(busDev, VL53L0X_REG_SYSRANGE_START, &vl53l0xRegByte, 1); + stepIssued = transferPending; + return RANGEFINDER_UPDATE_RETRY_US; + } + stepIssued = false; + if (vl53l0xRegByte & 0x01) { if (checkTimeoutExpired()) { lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE; measSteps = MEASUREMENT_START; } + return 0; // poll again on the next regular cycle } - else { - startTimeout(); - measSteps = MEASUREMENT_READ; + startTimeout(); + measSteps = MEASUREMENT_READ_STATUS; + FALLTHROUGH; + + case MEASUREMENT_READ_STATUS: + // Wait for data + if (!stepIssued) { + transferPending = busReadBufStart(busDev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &vl53l0xRegByte, 1); + stepIssued = transferPending; + return RANGEFINDER_UPDATE_RETRY_US; } - break; - - case MEASUREMENT_READ: - if ((readReg(rangefinder->busDev, VL53L0X_REG_RESULT_INTERRUPT_STATUS) & 0x07) == 0) { + stepIssued = false; + if ((vl53l0xRegByte & 0x07) == 0) { if (checkTimeoutExpired()) { lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE; measSteps = MEASUREMENT_START; } + return 0; // poll again on the next regular cycle + } + measSteps = MEASUREMENT_READ_RESULT; + FALLTHROUGH; + + case MEASUREMENT_READ_RESULT: + if (!stepIssued) { + transferPending = busReadBufStart(busDev, VL53L0X_REG_RESULT_RANGE_STATUS + 10, vl53l0xResult, sizeof(vl53l0xResult)); + stepIssued = transferPending; + return RANGEFINDER_UPDATE_RETRY_US; } - else { + stepIssued = false; + { // assumptions: Linearity Corrective Gain is 1000 (default); - uint16_t raw = readReg16(rangefinder->busDev, VL53L0X_REG_RESULT_RANGE_STATUS + 10); - writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01); - + const uint16_t raw = ((uint16_t)vl53l0xResult[0] << 8) | vl53l0xResult[1]; lastMeasurementCm = raw / 10; lastMeasurementIsNew = true; - measSteps = MEASUREMENT_START; } - break; - } -#else // CONTINUOUS MEASUREMENT MODE - switch (measSteps) { - case MEASUREMENT_START: - // Initiate new measurement - writeReg(rangefinder->busDev, 0x80, 0x01); - writeReg(rangefinder->busDev, 0xFF, 0x01); - writeReg(rangefinder->busDev, 0x00, 0x00); - writeReg(rangefinder->busDev, 0x91, stopVariable); - writeReg(rangefinder->busDev, 0x00, 0x01); - writeReg(rangefinder->busDev, 0xFF, 0x00); - writeReg(rangefinder->busDev, 0x80, 0x00); - - uint16_t osc_calibrate_val = readReg16(VL53L0X_REG_OSC_CALIBRATE_VAL); - - if (osc_calibrate_val != 0) { - writeReg32(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, 50 * osc_calibrate_val); + measSteps = MEASUREMENT_CLEAR; + FALLTHROUGH; + + case MEASUREMENT_CLEAR: + if (!stepIssued) { + transferPending = busWriteStart(busDev, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01); + stepIssued = transferPending; + return RANGEFINDER_UPDATE_RETRY_US; } - else { - writeReg32(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, 50); - } - - writeReg(rangefinder->busDev, VL53L0X_REG_SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK - - startTimeout(); - measSteps = MEASUREMENT_READ; - break; - - case MEASUREMENT_READ: - default: - if ((readReg(rangefinder->busDev, VL53L0X_REG_RESULT_INTERRUPT_STATUS) & 0x07) == 0) { - if (checkTimeoutExpired()) { - lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE; - - // Restart timeout - startTimeout(); - measSteps = MEASUREMENT_READ; - } - } - else { - // assumptions: Linearity Corrective Gain is 1000 (default); - uint16_t raw = readReg16(rangefinder->busDev, VL53L0X_REG_RESULT_RANGE_STATUS + 10); - writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01); - - lastMeasurementCm = raw / 10; - - // Restart timeout - startTimeout(); - measSteps = MEASUREMENT_READ; - } - break; + stepIssued = false; + measSteps = MEASUREMENT_START; + return 0; } -#endif + + return 0; } int32_t vl53l0x_GetDistance(rangefinderDev_t *dev) diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c index f4f95297ae7..2cd61121092 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c @@ -449,7 +449,7 @@ static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, /** * @brief This function returns the distance measured by the sensor in mm */ -static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance); +// VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance); /** * @brief This function returns the returned signal per SPAD in kcps/SPAD. @@ -723,6 +723,7 @@ static int32_t lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE; static bool lastMeasurementIsNew = false; static bool isInitialized = false; static bool isResponding = true; +static uint8_t interruptPolarity = 1; // read once at init, decides which GPIO__TIO_HV_STATUS level means data ready #define _I2CWrite(dev, data, size) \ (busWriteBuf(dev, 0xFF, data, size) ? 0 : -1) @@ -1232,16 +1233,17 @@ static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t // return status; // } -static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; - - status = (VL53L1_RdWord(dev, - VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp)); - *distance = tmp; - return status; -} +// The asynchronous read step fetches the distance register itself, kept for reference like the other unused API calls +// VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; +// +// status = (VL53L1_RdWord(dev, +// VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp)); +// *distance = tmp; +// return status; +// } // VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalRate) // { @@ -1613,26 +1615,110 @@ static void vl53l1x_Init(rangefinderDev_t * rangefinder) VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, 33); /* in ms possible values [20, 50, 100, 200, 500] */ VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, RANGEFINDER_VL53L1X_TASK_PERIOD_MS); /* in ms, IM must be > = TB */ status = VL53L1X_StartRanging(rangefinder->busDev); + VL53L1X_GetInterruptPolarity(rangefinder->busDev, &interruptPolarity); } isInitialized = (status == VL53L1_ERROR_NONE); } -void vl53l1x_Update(rangefinderDev_t * rangefinder) +/* + * Non-blocking measurement cycle. Every register access of this sensor is a raw write of the 16 bit index + * followed by a raw read, so a step is either "send index" or "read data" / "write register". + */ +typedef enum { + VL53L1X_STEP_STATUS_INDEX = 0, // send the index of GPIO__TIO_HV_STATUS + VL53L1X_STEP_STATUS_READ, // read it, data ready when the level matches the interrupt polarity + VL53L1X_STEP_RESULT_INDEX, // send the index of the range result + VL53L1X_STEP_RESULT_READ, // read the range + VL53L1X_STEP_CLEAR, // clear the interrupt, finishes while the task waits for the next cycle +} vl53l1xStep_e; + +// Buffers of the non-blocking transfers, they must outlive the call +static uint8_t vl53l1xIndex[2]; +static uint8_t vl53l1xRx[2]; +static const uint8_t vl53l1xClearInterrupt[3] = { SYSTEM__INTERRUPT_CLEAR >> 8, SYSTEM__INTERRUPT_CLEAR & 0xFF, 0x01 }; + +static bool vl53l1xSendIndex(busDevice_t * busDev, uint16_t index) { - uint16_t Distance; - uint8_t dataReady; + vl53l1xIndex[0] = index >> 8; + vl53l1xIndex[1] = index & 0xFF; + return busWriteBufStart(busDev, 0xFF, vl53l1xIndex, sizeof(vl53l1xIndex)); +} + +timeDelta_t vl53l1x_Update(rangefinderDev_t * rangefinder) +{ + static vl53l1xStep_e step = VL53L1X_STEP_STATUS_INDEX; + static bool transferPending = false; // a non-blocking transfer of the current step is on the bus + static bool stepIssued = false; // the current step started its transfer, the next call evaluates it + busDevice_t * busDev = rangefinder->busDev; + bool busError = false; if (!isInitialized) { - return; + return 0; + } + + if (transferPending) { + if (busIsBusy(busDev, &busError) && !busError) { + return RANGEFINDER_UPDATE_RETRY_US; + } + transferPending = false; + isResponding = !busError; + if (busError) { + step = VL53L1X_STEP_STATUS_INDEX; + stepIssued = false; + return 0; + } } - VL53L1X_CheckForDataReady(rangefinder->busDev, &dataReady); - if (dataReady != 0) { - VL53L1X_GetDistance(rangefinder->busDev, &Distance); - lastMeasurementCm = Distance / 10; - lastMeasurementIsNew = true; + switch (step) { + case VL53L1X_STEP_STATUS_INDEX: + transferPending = vl53l1xSendIndex(busDev, GPIO__TIO_HV_STATUS); + if (transferPending) { + step = VL53L1X_STEP_STATUS_READ; + } + return RANGEFINDER_UPDATE_RETRY_US; + + case VL53L1X_STEP_STATUS_READ: + if (!stepIssued) { + transferPending = busReadBufStart(busDev, 0xFF, vl53l1xRx, 1); + stepIssued = transferPending; + return RANGEFINDER_UPDATE_RETRY_US; + } + stepIssued = false; + if ((vl53l1xRx[0] & 1) == interruptPolarity) { + step = VL53L1X_STEP_RESULT_INDEX; + } else { + step = VL53L1X_STEP_CLEAR; // no new data this cycle, the interrupt is still cleared as before + } + return RANGEFINDER_UPDATE_RETRY_US; + + case VL53L1X_STEP_RESULT_INDEX: + transferPending = vl53l1xSendIndex(busDev, VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0); + if (transferPending) { + step = VL53L1X_STEP_RESULT_READ; + } + return RANGEFINDER_UPDATE_RETRY_US; + + case VL53L1X_STEP_RESULT_READ: + if (!stepIssued) { + transferPending = busReadBufStart(busDev, 0xFF, vl53l1xRx, 2); + stepIssued = transferPending; + return RANGEFINDER_UPDATE_RETRY_US; + } + stepIssued = false; + { + const uint16_t distanceMm = ((uint16_t)vl53l1xRx[0] << 8) | vl53l1xRx[1]; + lastMeasurementCm = distanceMm / 10; + lastMeasurementIsNew = true; + } + step = VL53L1X_STEP_CLEAR; + FALLTHROUGH; + + default: + // The clear write completes while the task waits for the next regular cycle + transferPending = busWriteBufStart(busDev, 0xFF, vl53l1xClearInterrupt, sizeof(vl53l1xClearInterrupt)); + step = VL53L1X_STEP_STATUS_INDEX; + return transferPending ? 0 : RANGEFINDER_UPDATE_RETRY_US; } - VL53L1X_ClearInterrupt(rangefinder->busDev); } int32_t vl53l1x_GetDistance(rangefinderDev_t *dev) diff --git a/src/main/drivers/sdcard/sdcard_spi.c b/src/main/drivers/sdcard/sdcard_spi.c index 6446ec09164..58c6040c4b2 100644 --- a/src/main/drivers/sdcard/sdcard_spi.c +++ b/src/main/drivers/sdcard/sdcard_spi.c @@ -63,7 +63,7 @@ static void sdcardSpi_deselect(void) //spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); int timeout = 100000; - while (busIsBusy(sdcard.dev)) { + while (busIsBusy(sdcard.dev, NULL)) { if (timeout-- == 0) { sdcard.failureCount++; if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) { @@ -872,7 +872,7 @@ void sdcardSpi_init(void) // Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles: int time = 100000; - while (busIsBusy(sdcard.dev)) { + while (busIsBusy(sdcard.dev, NULL)) { if (time-- == 0) { busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD); busDeviceDeInit(sdcard.dev); diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index be828d5007c..380371f727a 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -52,6 +52,7 @@ typedef bool (*sensorMagInitFuncPtr)(struct magDev_s *mag); typedef bool (*sensorMagReadFuncPtr)(struct magDev_s *mag); struct temperatureDev_s; typedef bool (*sensorTempReadFuncPtr)(struct temperatureDev_s *tempDev, int16_t *temperature); +typedef bool (*sensorTempReadStartFuncPtr)(struct temperatureDev_s *tempDev); struct opflowDev_s; typedef bool (*sensorOpflowInitFuncPtr)(struct opflowDev_s *mag); typedef bool (*sensorOpflowUpdateFuncPtr)(struct opflowDev_s *mag); diff --git a/src/main/drivers/temperature/lm75.c b/src/main/drivers/temperature/lm75.c index 98275e411a4..ec627072861 100644 --- a/src/main/drivers/temperature/lm75.c +++ b/src/main/drivers/temperature/lm75.c @@ -34,26 +34,31 @@ #ifdef USE_TEMPERATURE_LM75 -static bool lm75Read(temperatureDev_t *tempDev, int16_t *temperature) +// The two result bytes land in the device scratchpad, each of the up to 8 sensors has its own +static bool lm75ReadStart(temperatureDev_t *tempDev) { - uint8_t buf[2]; - - bool ack = busReadBuf(tempDev->busDev, LM75_TEMPERATURE_REG_ADDR, buf, 2); + uint8_t *buf = busDeviceGetScratchpadMemory(tempDev->busDev); + return busReadBufStart(tempDev->busDev, LM75_TEMPERATURE_REG_ADDR, buf, 2); +} - if (ack) { - if (temperature) *temperature = (int8_t)buf[0] * 10 + (buf[1] >> 7) * 5; - return true; - } +// Parses the result of the last lm75ReadStart(), the bus itself is not touched +static bool lm75Read(temperatureDev_t *tempDev, int16_t *temperature) +{ + const uint8_t *buf = busDeviceGetScratchpadMemory(tempDev->busDev); - return false; + if (temperature) *temperature = (int8_t)buf[0] * 10 + (buf[1] >> 7) * 5; + return true; } #define DETECTION_MAX_RETRY_COUNT 5 static bool deviceDetect(temperatureDev_t *tempDev) { + uint8_t buf[2]; + + // Blocking read: only a chip that acknowledges counts, lm75Read() alone cannot tell whether one is there for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { delay(10); - if (lm75Read(tempDev, NULL)) return true; + if (busReadBuf(tempDev->busDev, LM75_TEMPERATURE_REG_ADDR, buf, 2)) return true; } return false; @@ -73,6 +78,13 @@ bool lm75Detect(temperatureDev_t *tempDev, uint8_t partialAddress) return false; } + // Get the result buffer now, lm75ReadStart() runs from the scheduler and must not be the first to allocate it + if (busDeviceGetScratchpadMemory(tempDev->busDev) == NULL) { + busDeviceDeInit(tempDev->busDev); + return false; + } + + tempDev->readStart = lm75ReadStart; tempDev->read = lm75Read; return true; diff --git a/src/main/drivers/temperature/temperature.h b/src/main/drivers/temperature/temperature.h index f10001a687c..005baf1b2be 100644 --- a/src/main/drivers/temperature/temperature.h +++ b/src/main/drivers/temperature/temperature.h @@ -4,5 +4,6 @@ typedef struct temperatureDev_s { busDevice_t *busDev; + sensorTempReadStartFuncPtr readStart; // optional: start a non-blocking read, read() then only parses sensorTempReadFuncPtr read; } temperatureDev_t; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 15339c4f39e..54fea0fc076 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -238,7 +238,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "TERRAIN_NAV", "ESC", "FW_TURN", - "MAG" + "MAG", + "I2C" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 31143e92706..9ef22da8549 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -61,6 +61,7 @@ #include "io/beeper.h" #include "io/lights.h" #include "io/dashboard.h" +#include "drivers/display_ug2864hsweg01.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/osd.h" @@ -188,7 +189,11 @@ void taskProcessGPS(timeUs_t currentTimeUs) void taskUpdateCompass(timeUs_t currentTimeUs) { if (sensors(SENSOR_MAG)) { - compassUpdate(currentTimeUs); + // A non-blocking read asks to be revisited shortly, otherwise the regular period is restored + const uint32_t newDeadline = compassUpdate(currentTimeUs); + if (newDeadline != 0) { + rescheduleTask(TASK_SELF, newDeadline); + } } } #endif @@ -200,12 +205,16 @@ void taskUpdateBaro(timeUs_t currentTimeUs) return; } - const uint32_t newDeadline = baroUpdate(); + bool newSampleReady = false; + const uint32_t newDeadline = baroUpdate(&newSampleReady); if (newDeadline != 0) { rescheduleTask(TASK_SELF, newDeadline); } - updatePositionEstimator_BaroTopic(currentTimeUs); + // The estimator derives the climb rate from consecutive samples, feed it only when there is a new one + if (newSampleReady) { + updatePositionEstimator_BaroTopic(currentTimeUs); + } } #endif @@ -216,9 +225,15 @@ void taskUpdatePitot(timeUs_t currentTimeUs) return; } - pitotUpdate(); + // A non-blocking read asks to be revisited shortly, otherwise the regular period is restored + bool newSampleReady = false; + const uint32_t newDeadline = pitotUpdate(&newSampleReady); + if (newDeadline != 0) { + rescheduleTask(TASK_SELF, newDeadline); + } - if ( pitotIsHealthy()) { + // Feed the estimator only when the airspeed was recomputed, not on a retry round + if (newSampleReady && pitotIsHealthy()) { updatePositionEstimator_PitotTopic(currentTimeUs); } } @@ -251,7 +266,11 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs) void taskUpdateIrlock(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - irlockUpdate(); + // A non-blocking read asks to be revisited shortly, otherwise the regular period is restored + const uint32_t newDeadline = irlockUpdate(); + if (newDeadline != 0) { + rescheduleTask(TASK_SELF, newDeadline); + } } #endif @@ -271,6 +290,8 @@ void taskDashboardUpdate(timeUs_t currentTimeUs) { if (feature(FEATURE_DASHBOARD)) { dashboardUpdate(currentTimeUs); + // The OLED driver queues its bytes, drain the queue quickly while there is something to send + rescheduleTask(TASK_SELF, i2c_OLED_pump() ? OLED_PUMP_RETRY_US : TASK_PERIOD_HZ(10)); } } #endif @@ -595,7 +616,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_COMPASS] = { .taskName = "COMPASS", .taskFunc = taskUpdateCompass, - .desiredPeriod = TASK_PERIOD_HZ(10), // Compass is updated at 10 Hz + .desiredPeriod = TASK_PERIOD_HZ(COMPASS_UPDATE_RATE_HZ), .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif @@ -622,7 +643,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_PITOT] = { .taskName = "PITOT", .taskFunc = taskUpdatePitot, - .desiredPeriod = TASK_PERIOD_MS(20), + .desiredPeriod = TASK_PERIOD_HZ(PITOT_UPDATE_RATE_HZ), .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif @@ -640,7 +661,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_IRLOCK] = { .taskName = "IRLOCK", .taskFunc = taskUpdateIrlock, - .desiredPeriod = TASK_PERIOD_HZ(100), + .desiredPeriod = TASK_PERIOD_HZ(IRLOCK_UPDATE_RATE_HZ), .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 142f5f06c9b..e0c92770042 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -93,7 +93,7 @@ tables: "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "MAG_CALIB", "VTOL_TRANSITION", "VTOL_MC_PROTECT", "TERRAIN_NAV", "ESC", - "FW_TURN", "MAG"] + "FW_TURN", "MAG", "I2C"] - name: vtol_mc_protection_mode values: ["OFF", "NAV", "NAV_AND_STABILIZED"] - name: aux_operator diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c index bc132e524b6..ba5c506b20d 100644 --- a/src/main/io/displayport_oled.c +++ b/src/main/io/displayport_oled.c @@ -78,7 +78,7 @@ static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint1 static bool oledIsTransferInProgress(const displayPort_t *displayPort) { UNUSED(displayPort); - return false; + return i2c_OLED_isBusy(); } static int oledHeartbeat(displayPort_t *displayPort) @@ -95,7 +95,7 @@ static void oledResync(displayPort_t *displayPort) static uint32_t oledTxBytesFree(const displayPort_t *displayPort) { UNUSED(displayPort); - return UINT32_MAX; + return i2c_OLED_txBytesFree(); } static const displayPortVTable_t oledVTable = { diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index aaea9dda501..52fc584b225 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -38,7 +38,6 @@ #include "drivers/barometer/barometer_lps25h.h" #include "drivers/barometer/barometer_fake.h" #include "drivers/barometer/barometer_ms56xx.h" -#include "drivers/barometer/barometer_spl06.h" #include "drivers/barometer/barometer_dps310.h" #include "drivers/barometer/barometer_2smpb_02b.h" #include "drivers/barometer/barometer_msp.h" @@ -146,8 +145,9 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) FALLTHROUGH; case BARO_SPL06: -#if defined(USE_BARO_SPL06) || defined(USE_BARO_SPI_SPL06) - if (spl06Detect(dev)) { +#if defined(USE_BARO_SPL06) + // SPL06-001 is register compatible with the DPS310 and shares its chip ID, the DPS310 driver handles both + if (baroSPL06Detect(dev)) { baroHardware = BARO_SPL06; break; } @@ -262,45 +262,114 @@ bool baroInit(void) } typedef enum { - BAROMETER_NEEDS_SAMPLES = 0, - BAROMETER_NEEDS_CALCULATION + BARO_STATE_TEMPERATURE_START = 0, + BARO_STATE_TEMPERATURE_READ, + BARO_STATE_TEMPERATURE_SAMPLE, + BARO_STATE_PRESSURE_START, + BARO_STATE_PRESSURE_READ, + BARO_STATE_PRESSURE_SAMPLE, } barometerState_e; -uint32_t baroUpdate(void) +// Delay before looking at a non-blocking bus transfer again +#define BARO_STATE_STEP_DELAY_US 1000 + +/* + * Runs the measurement cycle described in drivers/barometer/barometer.h. + * Returns the delay until the next call (0 keeps the current task period). *newSampleReady is set when + * baro.baroPressure / baro.baroTemperature were updated in this call. + */ +uint32_t baroUpdate(bool *newSampleReady) { - static barometerState_e state = BAROMETER_NEEDS_SAMPLES; + static barometerState_e state = BARO_STATE_TEMPERATURE_START; + baroDev_t *dev = &baro.dev; + bool busError = false; + + *newSampleReady = false; #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { - return 0; + // Pressure and temperature are injected over MSP, keep publishing them at the sensor's pace + *newSampleReady = true; + return dev->up_delay; } #endif - switch (state) { - default: - case BAROMETER_NEEDS_SAMPLES: - if (baro.dev.get_ut) { - baro.dev.get_ut(&baro.dev); - } - if (baro.dev.start_up) { - baro.dev.start_up(&baro.dev); - } - state = BAROMETER_NEEDS_CALCULATION; - return baro.dev.up_delay; - break; - - case BAROMETER_NEEDS_CALCULATION: - if (baro.dev.get_up) { - baro.dev.get_up(&baro.dev); - } - if (baro.dev.start_ut) { - baro.dev.start_ut(&baro.dev); - } - //output: baro.baroPressure, baro.baroTemperature - baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature); - state = BAROMETER_NEEDS_SAMPLES; - return baro.dev.ut_delay; - break; + // States that need no wait on the bus run back to back within one call + for (;;) { + switch (state) { + default: + case BARO_STATE_TEMPERATURE_START: + if (dev->start_ut && !dev->start_ut(dev) && dev->read_ut) { + return BARO_STATE_STEP_DELAY_US; // non-blocking start refused, bus is busy + } + state = BARO_STATE_TEMPERATURE_READ; + return dev->ut_delay; + + case BARO_STATE_TEMPERATURE_READ: + if (dev->read_ut) { + if (!dev->read_ut(dev)) { + return BARO_STATE_STEP_DELAY_US; // bus is busy, try again shortly + } + state = BARO_STATE_TEMPERATURE_SAMPLE; + return BARO_STATE_STEP_DELAY_US; + } + state = BARO_STATE_TEMPERATURE_SAMPLE; // blocking driver, sample right away + break; + + case BARO_STATE_TEMPERATURE_SAMPLE: + if (dev->read_ut) { + if (dev->busDev && busIsBusy(dev->busDev, &busError) && !busError) { + return BARO_STATE_STEP_DELAY_US; // transfer still in progress + } + if (busError || (dev->get_ut && !dev->get_ut(dev))) { + state = BARO_STATE_TEMPERATURE_START; // transfer failed or sample unusable, redo the phase + break; + } + } + else if (dev->get_ut) { + dev->get_ut(dev); + } + state = BARO_STATE_PRESSURE_START; + break; + + case BARO_STATE_PRESSURE_START: + if (dev->start_up && !dev->start_up(dev) && dev->read_up) { + return BARO_STATE_STEP_DELAY_US; // non-blocking start refused, bus is busy + } + state = BARO_STATE_PRESSURE_READ; + return dev->up_delay; + + case BARO_STATE_PRESSURE_READ: + if (dev->read_up) { + if (!dev->read_up(dev)) { + return BARO_STATE_STEP_DELAY_US; // bus is busy, try again shortly + } + state = BARO_STATE_PRESSURE_SAMPLE; + return BARO_STATE_STEP_DELAY_US; + } + state = BARO_STATE_PRESSURE_SAMPLE; // blocking driver, sample right away + break; + + case BARO_STATE_PRESSURE_SAMPLE: + if (dev->read_up) { + if (dev->busDev && busIsBusy(dev->busDev, &busError) && !busError) { + return BARO_STATE_STEP_DELAY_US; // transfer still in progress + } + if (busError || (dev->get_up && !dev->get_up(dev))) { + state = BARO_STATE_PRESSURE_START; // transfer failed or sample unusable, redo the phase + break; + } + } + else if (dev->get_up) { + dev->get_up(dev); + } + + //output: baro.baroPressure, baro.baroTemperature + dev->calculate(dev, &baro.baroPressure, &baro.baroTemperature); + *newSampleReady = true; + state = dev->combined_read ? BARO_STATE_PRESSURE_START : BARO_STATE_TEMPERATURE_START; + break; + } } } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 091b3f26a4f..3d20e7151fb 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -61,7 +61,7 @@ PG_DECLARE(barometerConfig_t, barometerConfig); bool baroInit(void); bool baroIsCalibrationComplete(void); void baroStartCalibration(void); -uint32_t baroUpdate(void); +uint32_t baroUpdate(bool *newSampleReady); int32_t baroCalculateAltitude(void); int32_t baroGetLatestAltitude(void); int16_t baroGetTemperature(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 171c60337f1..5b36de982a4 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -456,9 +456,23 @@ static void checkBatteryCapacityState(void) } } +#if defined(USE_INA226) +// Keeps the cached INA226 readings fresh, the transfers run in the background +static void ina226Poll(void) +{ + if (ina226Detected) { + ina226Update(&ina226Dev); + } +} +#endif + void batteryUpdate(timeUs_t timeDelta) { static timeUs_t batteryConnectedTime = 0; + +#if defined(USE_INA226) + ina226Poll(); +#endif /* battery has just been connected*/ if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD) { if(batteryConnectedTime == 0) { @@ -756,6 +770,10 @@ void currentMeterUpdate(timeUs_t timeDelta) { static int64_t mAhdrawnRaw = 0; +#if defined(USE_INA226) + ina226Poll(); +#endif + switch (batteryMetersConfig()->current.type) { case CURRENT_SENSOR_ADC: { diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 79bcf167bbc..540cbc12bf6 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -430,13 +430,20 @@ static void compassApplyDetectedOrientation(int16_t rollDD, int16_t pitchDD, int compassRefreshAlignment(); } -void compassUpdate(timeUs_t currentTimeUs) +// Delay before looking at a non-blocking bus transfer again +#define COMPASS_READ_RETRY_US 1000 + +/* + * Reads the magnetometer and processes the sample. Returns the delay until the next call: the regular period once + * a sample was handled, a short retry while a non-blocking read is in progress. + */ +uint32_t compassUpdate(timeUs_t currentTimeUs) { #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { - magUpdatedAtLeastOnce = true; - return; - } + if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { + magUpdatedAtLeastOnce = true; + return COMPASS_UPDATE_PERIOD_US; + } #endif static sensorCalibrationState_t calState; static int16_t magPrev[XYZ_AXIS_COUNT]; @@ -458,11 +465,45 @@ void compassUpdate(timeUs_t currentTimeUs) } #endif + // Non-blocking drivers deliver a sample over one or more bus transfers spread across calls + static bool transferPending = false; // a transfer started by readStart() is on the bus + static bool lastTransferStarted = false; // the transfer in flight is the last one of the sample + + if (mag.dev.readStart) { + bool busError = false; + + if (transferPending && mag.dev.busDev && busIsBusy(mag.dev.busDev, &busError) && !busError) { + return COMPASS_READ_RETRY_US; // still on the bus + } + + if (busError) { + transferPending = false; + lastTransferStarted = false; + mag.magADC[X] = 0; + mag.magADC[Y] = 0; + mag.magADC[Z] = 0; + return COMPASS_UPDATE_PERIOD_US; + } + + if (!lastTransferStarted) { + const busReadStepResult_e result = mag.dev.readStart(&mag.dev, !transferPending); + if (result != BUS_READ_STEP_BUSY) { + transferPending = true; + lastTransferStarted = (result == BUS_READ_STEP_LAST); + } + return COMPASS_READ_RETRY_US; // wait for the transfer, or for the bus to free up + } + + // The last transfer of the sample is complete, read() only parses it + transferPending = false; + lastTransferStarted = false; + } + if (!mag.dev.read(&mag.dev)) { mag.magADC[X] = 0; mag.magADC[Y] = 0; mag.magADC[Z] = 0; - return; + return COMPASS_UPDATE_PERIOD_US; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -590,6 +631,8 @@ void compassUpdate(timeUs_t currentTimeUs) } magUpdatedAtLeastOnce = true; + + return COMPASS_UPDATE_PERIOD_US; } #endif diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index b277f6b919a..0ac33964048 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -77,9 +77,13 @@ typedef struct compassConfig_s { PG_DECLARE(compassConfig_t, compassConfig); +// Regular sample rate of the compass task +#define COMPASS_UPDATE_RATE_HZ 10 +#define COMPASS_UPDATE_PERIOD_US (1000000 / COMPASS_UPDATE_RATE_HZ) + bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); bool compassInit(void); -void compassUpdate(timeUs_t currentTimeUs); +uint32_t compassUpdate(timeUs_t currentTimeUs); bool compassIsReady(void); bool compassIsHealthy(void); bool compassIsCalibrationComplete(void); diff --git a/src/main/sensors/irlock.c b/src/main/sensors/irlock.c index 8484a78386a..3d578b038e2 100644 --- a/src/main/sensors/irlock.c +++ b/src/main/sensors/irlock.c @@ -66,10 +66,21 @@ bool irlockMeasurementIsValid(void) { return measurementValid; } -void irlockUpdate(void) +uint32_t irlockUpdate(void) { - if (irlockDetected && irlockDev.read(&irlockDev, &irlockData)) lastUpdateMs = millis(); + uint32_t nextCallUs = IRLOCK_UPDATE_PERIOD_US; + + if (irlockDetected) { + const irlockReadResult_e result = irlockDev.read(&irlockDev, &irlockData); + if (result == IRLOCK_READ_FRAME) { + lastUpdateMs = millis(); + } else if (result == IRLOCK_READ_PENDING) { + nextCallUs = IRLOCK_READ_RETRY_US; // a transfer is on the bus, come back shortly + } + } measurementValid = millis() - lastUpdateMs < IRLOCK_TIMEOUT; + + return nextCallUs; } #define X_TO_DISTANCE_FACTOR -0.0029387573f diff --git a/src/main/sensors/irlock.h b/src/main/sensors/irlock.h index f39da22aced..9626c294ef9 100644 --- a/src/main/sensors/irlock.h +++ b/src/main/sensors/irlock.h @@ -22,7 +22,12 @@ void irlockInit(void); bool irlockHasBeenDetected(void); -void irlockUpdate(void); +// Regular sample rate of the IRLock task and the retry delay while a transfer is on the bus +#define IRLOCK_UPDATE_RATE_HZ 100 +#define IRLOCK_UPDATE_PERIOD_US (1000000 / IRLOCK_UPDATE_RATE_HZ) +#define IRLOCK_READ_RETRY_US 1000 + +uint32_t irlockUpdate(void); // returns the delay until the next call bool irlockMeasurementIsValid(void); bool irlockGetPosition(float *distX, float *distY); diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index c380064c757..64438d770a7 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -235,6 +235,9 @@ static void performPitotCalibrationCycle(void) } } +// Set by pitotThread() when a non-blocking read is on the bus and the task should come back shortly +static bool pitotRetrySoon = false; + STATIC_PROTOTHREAD(pitotThread) { ptBegin(pitotThread); @@ -258,7 +261,43 @@ STATIC_PROTOTHREAD(pitotThread) } } - if ((millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) { + if (pitot.dev.readStart) { + // Non-blocking driver: the sample arrives over one or more bus transfers spread across calls + static bool transferPending = false; // a transfer started by readStart() is on the bus + static bool lastTransferStarted = false; // the transfer in flight is the last one of the sample + bool busError = false; + + if (transferPending && pitot.dev.busDev && busIsBusy(pitot.dev.busDev, &busError) && !busError) { + pitotRetrySoon = true; // still on the bus + } + else if (busError) { + transferPending = false; // failed sample, the next attempt comes after the regular delay + lastTransferStarted = false; + } + else if (lastTransferStarted) { + transferPending = false; // the last transfer is complete, get() only parses it + lastTransferStarted = false; + if (pitot.dev.get(&pitot.dev)) { // read current data + pitot.lastSeenHealthyMs = millis(); + } + pitot.dev.start(&pitot.dev); // init for next read; only a parsed sample counts as healthy + } + else if (transferPending || (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) { + const busReadStepResult_e result = pitot.dev.readStart(&pitot.dev, !transferPending); + if (result != BUS_READ_STEP_BUSY) { + transferPending = true; + lastTransferStarted = (result == BUS_READ_STEP_LAST); + } + pitotRetrySoon = true; // wait for the transfer, or for the bus to free up + } + + if (pitotRetrySoon) { + // Nothing new this round, keep the last airspeed and look at the bus again shortly + ptYield(); + continue; + } + } + else if ((millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) { if (pitot.dev.get(&pitot.dev)) { // read current data pitot.lastSeenHealthyMs = millis(); } @@ -366,8 +405,10 @@ static float getVirtualAirspeedEstimate(void) return pidProfile()->fixedWingReferenceAirspeed; //float cm/s } -void pitotUpdate(void) +uint32_t pitotUpdate(bool *newSampleReady) { + pitotRetrySoon = false; + #ifdef USE_SIMULATOR if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { if (!SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE)) { @@ -388,6 +429,11 @@ void pitotUpdate(void) // Check pitot airspeed validity and cache result for external use pitotAirspeedValidCached = isPitotAirspeedValid(); + + // A retry round leaves the airspeed untouched, nothing new for the consumers + *newSampleReady = !pitotRetrySoon; + + return pitotRetrySoon ? PITOT_READ_RETRY_US : PITOT_UPDATE_PERIOD_US; } /* diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 21a823329c1..f9d0c06a3a0 100755 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -64,10 +64,15 @@ typedef struct pito_s { extern pitot_t pitot; +// Regular sample rate of the pitot task and the retry delay while a non-blocking read is on the bus +#define PITOT_UPDATE_RATE_HZ 50 +#define PITOT_UPDATE_PERIOD_US (1000000 / PITOT_UPDATE_RATE_HZ) +#define PITOT_READ_RETRY_US 1000 + bool pitotInit(void); bool pitotIsCalibrationComplete(void); void pitotStartCalibration(void); -void pitotUpdate(void); +uint32_t pitotUpdate(bool *newSampleReady); // returns the delay until the next call, *newSampleReady is set when airspeed was recomputed float getAirspeedEstimate(void); bool pitotIsHealthy(void); bool pitotGetValidForAirspeed(void); diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index a721bb98634..b339778f606 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -227,11 +227,14 @@ static int32_t applyMedianFilter(int32_t newReading) */ timeDelta_t rangefinderUpdate(void) { + timeDelta_t nextCallUs = 0; + if (rangefinder.dev.update) { - rangefinder.dev.update(&rangefinder.dev); + // A driver with a non-blocking transfer on the bus asks to be called again shortly + nextCallUs = rangefinder.dev.update(&rangefinder.dev); } - return MS2US(rangefinder.dev.delayMs); + return (nextCallUs > 0) ? nextCallUs : (timeDelta_t)MS2US(rangefinder.dev.delayMs); } /** diff --git a/src/main/sensors/temperature.c b/src/main/sensors/temperature.c index fe5e80c792f..ab84b29dccd 100644 --- a/src/main/sensors/temperature.c +++ b/src/main/sensors/temperature.c @@ -191,6 +191,12 @@ bool tempSensorStringToAddress(const char *hex_address, uint64_t *address) static uint8_t temperatureUpdateSensorIndex; static bool temperatureUpdateValueValid; +#ifdef USE_TEMPERATURE_LM75 +// Protothread state: locals do not survive a ptWait(), so the sensor being read lives here +static temperatureDev_t *temperatureUpdateDev; +static bool temperatureBusError; +#endif + #ifdef DS18B20_DRIVER_AVAILABLE static uint8_t temperatureUpdateIndex; static uint8_t temperatureUpdateBuf[9]; @@ -228,14 +234,23 @@ PROTOTHREAD(temperatureUpdate) #ifdef USE_TEMPERATURE_LM75 if (configSlot->type == TEMP_SENSOR_LM75) { if (configSlot->address < 8) { - temperatureDev_t *dev = lm75Dev + configSlot->address; - if (dev->read && dev->read(dev, &tempSensorValue[temperatureUpdateSensorIndex])) temperatureUpdateValueValid = true; + temperatureUpdateDev = lm75Dev + configSlot->address; + if (temperatureUpdateDev->readStart) { + // non-blocking read: start it, wait for the bus, then parse + if (temperatureUpdateDev->readStart(temperatureUpdateDev)) { + ptWait(!busIsBusy(temperatureUpdateDev->busDev, &temperatureBusError)); + if (!temperatureBusError && temperatureUpdateDev->read(temperatureUpdateDev, &tempSensorValue[temperatureUpdateSensorIndex])) temperatureUpdateValueValid = true; + } + } else if (temperatureUpdateDev->read && temperatureUpdateDev->read(temperatureUpdateDev, &tempSensorValue[temperatureUpdateSensorIndex])) { + temperatureUpdateValueValid = true; + } } } #endif #ifdef DS18B20_DRIVER_AVAILABLE - if ((configSlot->type == TEMP_SENSOR_DS18B20) && owDev) { + // configSlot is stale once the LM75 ptWait() above has resumed, read the slot again + if ((tempSensorConfig(temperatureUpdateSensorIndex)->type == TEMP_SENSOR_DS18B20) && owDev) { bool ack = owDev->owResetCommand(owDev); if (!ack) goto temperatureUpdateError; ptWait(owDev->owBusReady(owDev)); diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index de97812ddcc..294d53f00ac 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -213,6 +213,9 @@ set_property(SOURCE pitotmeter_virtual_validity_unittest.cc PROPERTY definitions set_property(SOURCE pitotmeter_failed_hw_validity_unittest.cc PROPERTY depends "sensors/pitotmeter.c" "common/maths.c" "common/filter.c" "common/calibration.c" "common/lulu.c") set_property(SOURCE pitotmeter_failed_hw_validity_unittest.cc PROPERTY definitions USE_PITOT USE_PITOT_VIRTUAL USE_WIND_ESTIMATOR USE_GPS) +set_property(SOURCE pitotmeter_async_read_unittest.cc PROPERTY depends "sensors/pitotmeter.c" "common/maths.c" "common/filter.c" "common/calibration.c" "common/lulu.c") +set_property(SOURCE pitotmeter_async_read_unittest.cc PROPERTY definitions USE_PITOT USE_PITOT_VIRTUAL USE_WIND_ESTIMATOR USE_GPS) + set_property(SOURCE servos_swap_roll_yaw_unittest.cc PROPERTY depends "flight/servos.c" "common/maths.c" "common/filter.c" "common/lulu.c" "programming/logic_condition.c") diff --git a/src/test/unit/battery_ina226_unittest.cc b/src/test/unit/battery_ina226_unittest.cc index 462af3c7f30..7bdf5be35b1 100644 --- a/src/test/unit/battery_ina226_unittest.cc +++ b/src/test/unit/battery_ina226_unittest.cc @@ -46,6 +46,15 @@ static busDevice_t fakeBusDevice; static uint16_t fakeRegisters[256]; static bool fakeRegisterReadable[256]; static uint32_t fakeFeatures; + +// Fake non-blocking bus (see ina226_unittest.cc): one transfer in flight, busy for one poll, then completed +// into the caller's buffer or failed when the register is unreadable; the failure stays reported until +// the next transfer is started, like the real I2C driver +static bool fakeTransferPending; +static bool fakeLastTransferFailed; +static uint8_t fakeTransferReg; +static uint8_t *fakeTransferData; +static int fakeBusyPolls; static timeUs_t fakeTimeUs; extern "C" { @@ -76,6 +85,12 @@ static void resetFakeBus(void) fakeBusDevice.busType = BUSTYPE_I2C; setFakeRegister(INA226_REG_MANUFACTURER_ID, 0x5449); setFakeRegister(INA226_REG_DIE_ID, 0x2260); + + fakeTransferPending = false; + fakeLastTransferFailed = false; + fakeTransferReg = 0; + fakeTransferData = nullptr; + fakeBusyPolls = 0; } static void clearFakeRegister(uint8_t reg) @@ -83,16 +98,20 @@ static void clearFakeRegister(uint8_t reg) fakeRegisterReadable[reg] = false; } +// Each INA226 register takes two polls (start, complete) and the two registers alternate, +// so a new value reaches the filters after a few rounds at the latest +#define SETTLE_ROUNDS 12 + static void settleVoltage(void) { - for (int i = 0; i < 5; i++) { + for (int i = 0; i < SETTLE_ROUNDS; i++) { batteryUpdate(10000000); } } static void settleCurrent(void) { - for (int i = 0; i < 5; i++) { + for (int i = 0; i < SETTLE_ROUNDS; i++) { currentMeterUpdate(10000000); } } @@ -170,6 +189,46 @@ extern "C" { return true; } + bool busReadBufStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) + { + EXPECT_EQ(&fakeBusDevice, busdev); + EXPECT_EQ(2, length); + + if (fakeTransferPending) { + return false; + } + + fakeTransferPending = true; + fakeLastTransferFailed = false; + fakeTransferReg = reg; + fakeTransferData = data; + fakeBusyPolls = 1; + return true; + } + + bool busIsBusy(const busDevice_t *busdev, bool *error) + { + EXPECT_EQ(&fakeBusDevice, busdev); + + if (fakeTransferPending && fakeBusyPolls-- > 0) { + *error = false; + return true; + } + + if (fakeTransferPending) { + fakeTransferPending = false; + if (fakeRegisterReadable[fakeTransferReg]) { + fakeTransferData[0] = fakeRegisters[fakeTransferReg] >> 8; + fakeTransferData[1] = fakeRegisters[fakeTransferReg] & 0xFF; + } else { + fakeLastTransferFailed = true; + } + } + + *error = fakeLastTransferFailed; + return false; + } + uint16_t adcGetChannel(uint8_t) { return 0; @@ -283,6 +342,8 @@ TEST(BatteryINA226, SamplesBatteryVoltageDirectlyFromIna226BusVoltage) resetBatteryTestState(); setFakeRegister(INA226_REG_BUS_VOLTAGE, 9600); + settleCurrent(); // the current meter polls the INA226 too, the battery voltage itself stays untouched + EXPECT_EQ(1200, getBatteryVoltageSample()); EXPECT_EQ(0, getBatteryRawVoltage()); } @@ -303,6 +364,8 @@ TEST(BatteryINA226, SamplesCurrentDirectlyFromConfiguredIna226Shunt) batteryMetersConfigMutable()->ina226.shuntResistanceMicroOhm = 300; setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 1200); + settleVoltage(); // the battery update polls the INA226 too, the amperage itself stays untouched + EXPECT_EQ(1000, getAmperageSample()); EXPECT_EQ(0, getAmperage()); } @@ -325,6 +388,8 @@ TEST(BatteryINA226, IgnoresAdcVoltageOffsetForIna226Current) batteryMetersConfigMutable()->ina226.shuntResistanceMicroOhm = 300; setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 1200); + settleVoltage(); // refresh the cached reading without touching the amperage + EXPECT_EQ(1000, getAmperageSample()); settleCurrent(); diff --git a/src/test/unit/ina226_unittest.cc b/src/test/unit/ina226_unittest.cc index fa4c461b25f..700aa5b4eb3 100644 --- a/src/test/unit/ina226_unittest.cc +++ b/src/test/unit/ina226_unittest.cc @@ -32,6 +32,17 @@ static bool fakeRegisterReadable[256]; static bool fakeInitSucceeds; static bool fakeDeInitCalled; +// Fake non-blocking bus: one transfer in flight at a time. busReadBufStart() queues it, busIsBusy() reports +// it busy for one poll and then completes it into the caller's buffer, or fails it when the register is +// unreadable. Like the real I2C driver, the failure stays reported until the next transfer is started. +static bool fakeTransferPending; +static bool fakeLastTransferFailed; +static bool fakeBusTaken; // another device holds the bus, starts are refused +static uint8_t fakeTransferReg; +static uint8_t *fakeTransferData; +static int fakeBusyPolls; +static int fakeTransferStarts; + static void resetFakeBus(void) { memset(&fakeBusDevice, 0, sizeof(fakeBusDevice)); @@ -41,6 +52,14 @@ static void resetFakeBus(void) fakeBusDevice.busType = BUSTYPE_I2C; fakeInitSucceeds = true; fakeDeInitCalled = false; + + fakeTransferPending = false; + fakeLastTransferFailed = false; + fakeBusTaken = false; + fakeTransferReg = 0; + fakeTransferData = nullptr; + fakeBusyPolls = 0; + fakeTransferStarts = 0; } static void setFakeRegister(uint8_t reg, uint16_t value) @@ -49,6 +68,13 @@ static void setFakeRegister(uint8_t reg, uint16_t value) fakeRegisterReadable[reg] = true; } +static void pumpUpdates(ina226Dev_t *dev, int calls) +{ + for (int i = 0; i < calls; i++) { + ina226Update(dev); + } +} + extern "C" { busDevice_t *busDeviceInit(busType_e bus, devHardwareType_e hw, uint8_t tag, resourceOwner_e owner) { @@ -79,6 +105,47 @@ extern "C" { data[1] = fakeRegisters[reg] & 0xFF; return true; } + + bool busReadBufStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) + { + EXPECT_EQ(&fakeBusDevice, busdev); + EXPECT_EQ(2, length); + + if (fakeTransferPending || fakeBusTaken) { + return false; + } + + fakeTransferPending = true; + fakeLastTransferFailed = false; + fakeTransferReg = reg; + fakeTransferData = data; + fakeBusyPolls = 1; + fakeTransferStarts++; + return true; + } + + bool busIsBusy(const busDevice_t *busdev, bool *error) + { + EXPECT_EQ(&fakeBusDevice, busdev); + + if (fakeTransferPending && fakeBusyPolls-- > 0) { + *error = false; + return true; + } + + if (fakeTransferPending) { + fakeTransferPending = false; + if (fakeRegisterReadable[fakeTransferReg]) { + fakeTransferData[0] = fakeRegisters[fakeTransferReg] >> 8; + fakeTransferData[1] = fakeRegisters[fakeTransferReg] & 0xFF; + } else { + fakeLastTransferFailed = true; + } + } + + *error = fakeLastTransferFailed; + return false; + } } TEST(INA226, DetectsExpectedIds) @@ -118,6 +185,27 @@ TEST(INA226, RejectsReadFailureDuringDetection) EXPECT_TRUE(fakeDeInitCalled); } +TEST(INA226, SeedsReadingsSynchronouslyAtInit) +{ + resetFakeBus(); + setFakeRegister(INA226_REG_MANUFACTURER_ID, 0x5449); + setFakeRegister(INA226_REG_DIE_ID, 0x2260); + setFakeRegister(INA226_REG_BUS_VOLTAGE, 9600); + setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 8000); + + ina226Dev_t dev = {}; + ASSERT_TRUE(ina226Init(&dev, 1, INA226_DEFAULT_I2C_ADDRESS)); + + // Both readings are available before the background sampling has run + uint16_t centiVolts = 0; + int16_t centiAmps = 0; + EXPECT_EQ(0, fakeTransferStarts); + EXPECT_TRUE(ina226ReadBusVoltage(&dev, ¢iVolts)); + EXPECT_EQ(1200, centiVolts); + EXPECT_TRUE(ina226ReadShuntCurrent(&dev, 2000, ¢iAmps)); + EXPECT_EQ(1000, centiAmps); +} + TEST(INA226, ConvertsBusVoltage) { EXPECT_EQ(0, ina226BusVoltageToCentivolts(0)); @@ -136,7 +224,7 @@ TEST(INA226, RejectsInvalidOrDisabledI2cConfiguration) EXPECT_FALSE(ina226Init(&dev, 1, 0x50)); } -TEST(INA226, ReadsBusVoltageBigEndian) +TEST(INA226, ReadsBusVoltageOverNonBlockingTransfer) { resetFakeBus(); setFakeRegister(INA226_REG_BUS_VOLTAGE, 9600); @@ -145,8 +233,41 @@ TEST(INA226, ReadsBusVoltageBigEndian) dev.busDev = &fakeBusDevice; uint16_t centiVolts = 0; + // First update starts the bus voltage read, nothing is available yet + ina226Update(&dev); + EXPECT_EQ(1, fakeTransferStarts); + EXPECT_EQ(INA226_REG_BUS_VOLTAGE, fakeTransferReg); + EXPECT_FALSE(ina226ReadBusVoltage(&dev, ¢iVolts)); + + // Still on the bus: no second transfer may be started + ina226Update(&dev); + EXPECT_EQ(1, fakeTransferStarts); + EXPECT_FALSE(ina226ReadBusVoltage(&dev, ¢iVolts)); + + // Completed: parsed big endian, and the rotation moves on to the shunt register + ina226Update(&dev); EXPECT_TRUE(ina226ReadBusVoltage(&dev, ¢iVolts)); EXPECT_EQ(1200, centiVolts); + EXPECT_EQ(2, fakeTransferStarts); + EXPECT_EQ(INA226_REG_SHUNT_VOLTAGE, fakeTransferReg); +} + +TEST(INA226, ReadsNegativeShuntCurrentOverNonBlockingTransfer) +{ + resetFakeBus(); + setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 0xE0C0); + + ina226Dev_t dev = {}; + dev.busDev = &fakeBusDevice; + dev.lastReg = INA226_REG_BUS_VOLTAGE; // shunt voltage is next in the rotation + int16_t centiAmps = 0; + + ina226Update(&dev); // shunt read started + EXPECT_EQ(INA226_REG_SHUNT_VOLTAGE, fakeTransferReg); + + pumpUpdates(&dev, 2); // busy, complete + EXPECT_TRUE(ina226ReadShuntCurrent(&dev, 2000, ¢iAmps)); + EXPECT_EQ(-1000, centiAmps); } TEST(INA226, ConvertsShuntVoltageToCurrent) @@ -168,28 +289,53 @@ TEST(INA226, SaturatesCurrentToInt16Range) EXPECT_EQ(INT16_MIN, ina226ShuntVoltageToCentiamps(INT16_MIN, 1)); } -TEST(INA226, ReadsNegativeShuntCurrentBigEndian) +TEST(INA226, FailedTransferInvalidatesReading) { resetFakeBus(); - setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 0xE0C0); + setFakeRegister(INA226_REG_BUS_VOLTAGE, 9600); ina226Dev_t dev = {}; dev.busDev = &fakeBusDevice; + uint16_t centiVolts = 0; int16_t centiAmps = 0; - EXPECT_TRUE(ina226ReadShuntCurrent(&dev, 2000, ¢iAmps)); - EXPECT_EQ(-1000, centiAmps); + pumpUpdates(&dev, 3); // bus voltage read completes, shunt read starts + ASSERT_TRUE(ina226ReadBusVoltage(&dev, ¢iVolts)); + EXPECT_FALSE(ina226ReadShuntCurrent(&dev, 2000, ¢iAmps)); // never read successfully + + // The device stops answering: the next bus voltage read fails and the stale value is dropped + fakeRegisterReadable[INA226_REG_BUS_VOLTAGE] = false; + pumpUpdates(&dev, 4); // shunt read fails, bus voltage read starts, busy, fails (and the shunt read starts again) + EXPECT_EQ(4, fakeTransferStarts); + EXPECT_FALSE(ina226ReadBusVoltage(&dev, ¢iVolts)); + EXPECT_FALSE(ina226ReadShuntCurrent(&dev, 2000, ¢iAmps)); + + // Readings come back once the device answers again + fakeRegisterReadable[INA226_REG_BUS_VOLTAGE] = true; + pumpUpdates(&dev, 4); // shunt read fails, bus voltage read starts, busy, completes + EXPECT_TRUE(ina226ReadBusVoltage(&dev, ¢iVolts)); + EXPECT_EQ(1200, centiVolts); } -TEST(INA226, ReportsReadFailures) +TEST(INA226, RetriesWhenBusStartIsRefused) { resetFakeBus(); + setFakeRegister(INA226_REG_BUS_VOLTAGE, 9600); ina226Dev_t dev = {}; dev.busDev = &fakeBusDevice; - uint16_t centiVolts = 1; - int16_t centiAmps = 1; + uint16_t centiVolts = 0; + // Another device holds the bus: nothing is started and nothing is pending + fakeBusTaken = true; + pumpUpdates(&dev, 2); + EXPECT_EQ(0, fakeTransferStarts); EXPECT_FALSE(ina226ReadBusVoltage(&dev, ¢iVolts)); - EXPECT_FALSE(ina226ReadShuntCurrent(&dev, 2000, ¢iAmps)); + + // The read starts on the next call once the bus is free + fakeBusTaken = false; + pumpUpdates(&dev, 3); + EXPECT_EQ(2, fakeTransferStarts); + EXPECT_TRUE(ina226ReadBusVoltage(&dev, ¢iVolts)); + EXPECT_EQ(1200, centiVolts); } diff --git a/src/test/unit/pitotmeter_failed_hw_validity_unittest.cc b/src/test/unit/pitotmeter_failed_hw_validity_unittest.cc index 3dacd68a658..4ca0c44973f 100644 --- a/src/test/unit/pitotmeter_failed_hw_validity_unittest.cc +++ b/src/test/unit/pitotmeter_failed_hw_validity_unittest.cc @@ -81,6 +81,7 @@ extern "C" { timeMs_t millis(void) { return fakeMillis; } timeUs_t micros(void) { return (timeUs_t)fakeMillis * 1000; } bool isEstimatedWindSpeedValid(void) { return windValid; } + bool busIsBusy(const busDevice_t *, bool *error) { *error = false; return false; } // the fake device has no readStart, never reached static bool fakeStartGet(pitotDev_t *) { return true; } static void fakeCalculate(pitotDev_t *, float *pressure, float *temperature) { @@ -97,7 +98,7 @@ static float pressureForCmS(float cmS) { class FailedHardwarePitotValidity : public ::testing::Test { protected: - void tick() { fakeMillis += 20; pitotUpdate(); } + void tick() { fakeMillis += 20; bool newSampleReady; pitotUpdate(&newSampleReady); } void SetUp() override { memset(&gpsSol, 0, sizeof(gpsSol)); diff --git a/src/test/unit/pitotmeter_virtual_validity_unittest.cc b/src/test/unit/pitotmeter_virtual_validity_unittest.cc index bde5ec7dbbf..677dfa696b4 100644 --- a/src/test/unit/pitotmeter_virtual_validity_unittest.cc +++ b/src/test/unit/pitotmeter_virtual_validity_unittest.cc @@ -83,6 +83,7 @@ extern "C" { timeMs_t millis(void) { return fakeMillis; } timeUs_t micros(void) { return (timeUs_t)fakeMillis * 1000; } bool isEstimatedWindSpeedValid(void) { return windValid; } + bool busIsBusy(const busDevice_t *, bool *error) { *error = false; return false; } // the fake device has no readStart, never reached } class VirtualPitotValidity : public ::testing::Test { @@ -100,7 +101,8 @@ class VirtualPitotValidity : public ::testing::Test { } bool airspeedValid() { - pitotUpdate(); // virtual pitot: sets lastSeenHealthyMs, refreshes validity cache + bool newSampleReady; + pitotUpdate(&newSampleReady); // virtual pitot: sets lastSeenHealthyMs, refreshes validity cache return pitotGetValidForAirspeed(); } };