diff --git a/.gitignore b/.gitignore index b0e52111..5511fab7 100644 --- a/.gitignore +++ b/.gitignore @@ -34,6 +34,9 @@ web/print/files/ .pio .pio-home compile_commands.json +# Written by the PlatformIO VS Code extension; machine-specific paths +.vscode/ +.gitignore.optional # Prerequisites *.d diff --git a/docs/api.md b/docs/api.md index 343a52cf..f0fa8058 100644 --- a/docs/api.md +++ b/docs/api.md @@ -120,7 +120,12 @@ curl -H "Authorization: Bearer YOUR_TOKEN" http://tiny-engineer.local/health "provisioning": false, "setup_ap_ssid": "", "setup_ap_ip": "", - "oled": true + "oled": true, + "i2c": { + "ok": true, + "pca9685": { "ok": true, "faults": 0, "recoveries": 0, "last_fault_ms": 0 }, + "oled": { "ok": true, "faults": 0, "recoveries": 0, "last_fault_ms": 0 } + } } ``` @@ -140,7 +145,13 @@ curl -H "Authorization: Bearer YOUR_TOKEN" http://tiny-engineer.local/health | `provisioning` | `true` when setup AP mode is active | | `setup_ap_ssid` | Setup AP SSID when `provisioning` is true, else `""` | | `setup_ap_ip` | Setup AP IP when `provisioning` is true (usually `192.168.4.1`), else `""` | -| `oled` | SSD1306 probed and initialized | +| `oled` | SSD1306 probed and initialized. Goes `false` while an I2C fault keeps the display unreachable | +| `i2c.ok` | `false` while either chip on the shared I2C bus cannot be used. The firmware checks each every 500 ms and sets a faulted chip up again, retrying with a growing delay (1 s up to 30 s) | +| `i2c.pca9685.ok` | Servo driver usable: reachable, holding its setup (awake, internal clock, auto-increment, output mode, prescale), and its last write succeeded. Goes `false` at the first failed servo write; servo writes and outputs stay off until the chip is set up again | +| `i2c.oled.ok` | Same value as `oled`. Stays `false`, with zero counts, on a robot that booted without a display | +| `i2c..faults` | Faults on that chip since boot. A bus fault that takes both chips down counts once on each | +| `i2c..recoveries` | Faults on that chip the firmware recovered from since boot | +| `i2c..last_fault_ms` | `uptime_ms` at that chip's latest fault; `0` when there has been none | ### `GET /settings` @@ -540,7 +551,7 @@ tiny-engineer park --transport serial { "ok": true, "park": "servos", "hold": true } ``` -Pose NVS write failure → **400** `{"ok":false,"error":"pose save failed"}` (PWM hold is still armed). Details: [hardware/servos.md](hardware/servos.md#flash-safe-park). +Pose NVS write failure → **400** `{"ok":false,"error":"pose save failed"}` (PWM hold is still armed). Servo driver faulted (`i2c.pca9685.ok` false in [`GET /health`](#get-health)) → **503** `{"ok":false,"error":"servo driver down"}`: the joints did not move, so the pose is not saved and no PWM hold is armed. Details: [hardware/servos.md](hardware/servos.md#flash-safe-park). ### `POST /test/servo` diff --git a/docs/hardware-for-software-engineers/08-tools-debugging-and-embedded-workflow.md b/docs/hardware-for-software-engineers/08-tools-debugging-and-embedded-workflow.md index 35082317..dbf6391b 100644 --- a/docs/hardware-for-software-engineers/08-tools-debugging-and-embedded-workflow.md +++ b/docs/hardware-for-software-engineers/08-tools-debugging-and-embedded-workflow.md @@ -86,7 +86,7 @@ Failure table and expected boot sequence: [testing.md](../hardware/testing.md). Once on Wi-Fi, the robot exposes **integration-test-style endpoints**: -- `/health` — alive, CPU temp +- `/health` — alive, CPU temp, I2C fault and recovery counts - `/test/servo` — single joint - Web UI — hardware tests without memorizing curl diff --git a/docs/hardware/interfaces.md b/docs/hardware/interfaces.md index 7737c056..67682f60 100644 --- a/docs/hardware/interfaces.md +++ b/docs/hardware/interfaces.md @@ -11,12 +11,27 @@ Shared bus. | SDA | GPIO0 | GP0 → PCA9685 SDA, OLED SDA | | SCL | GPIO1 | GP1 → PCA9685 SCL, Waveshare OLED **SCL** | -| Device | Address | Firmware behaviour if missing | +| Device | Address | Firmware behaviour if missing at boot | | --- | --- | --- | | PCA9685 | **`0x40`** | Hard fail: hang, 1 red blink (see [blink codes](testing.md#boot-failure-blink-codes)) | | SSD1306 OLED | **`0x3C`** | Soft fail: tests continue without display | -Bus start: `Wire.begin(I2C_SDA, I2C_SCL)` with no explicit frequency → Arduino-ESP32 default (typically **100 kHz**). Not set in this repo. +That table is the boot check. After boot the firmware keeps watching both devices (`src/hardware/i2c_health.cpp`): + +- **Check:** every 500 ms the OLED must answer, and the PCA9685 must answer, be awake on its internal clock with auto-increment on, and still hold its output mode (`MODE2 = 0x04`) and prescale. Two failed checks in a row are a fault. Each chip is tracked separately, so one that stays down does not stop the checks on the other. A healthy PCA9685 check is two register reads: `MODE1` and `MODE2` together, then the prescale. +- **Failed servo write:** a fault on its own. The first one takes the PCA9685 down at once and switches its outputs off, even inside a blocking move that never reaches the check. So does a boot setup that does not read back correctly. +- **While down:** writes to the faulted chip are skipped, so failed transfers do not stall audio and HTTP. At a PCA9685 fault the servo outputs are switched off (OE), so a corrupted setup cannot keep driving them. That needs OE driven by the firmware, the default; with `PCA9685_OE_WIRED = false` a fault cannot switch the outputs off ([PCA9685 output enable](#pca9685-output-enable)). +- **PCA9685 recovery:** if the chip does not answer, restart `Wire` and clock out a device left holding SDA first. A chip that kept its setup only lost a write or a read. A chip that lost its setup lost power or was corrupted: set it up again, and set the OLED up again too afterwards, since both share a supply. Either way, the outputs come back on with every servo channel off, and then every channel is written and read back. The PCA9685 applies channel values at the end of a LOW cycle, so each servo starts on a whole pulse; turning OE on mid-pulse would cut one short. Servos jump to where the firmware thinks they are. Outputs stay disabled unless the setup and every channel read back correctly. +- **OLED recovery:** send the panel its setup and the last frame, so a static screen survives too. Every transfer is checked, since the SSD1306 driver ignores failed writes and a half-configured panel still answers its address. The current rotation is kept. Never restarts the bus, so an OLED fault alone does not disturb the servos. +- **Audio:** a recovery runs in steps, one per loop pass, and a pass services at most one chip. Transfers time out after 20 ms (Wire's default is 50 ms), so a pass costs about one timeout at worst, well inside the speaker's DMA buffer of about 65 ms. +- **Retry:** a failed attempt is retried after 1 s, doubling up to 30 s. +- **Report:** `[i2c]` lines on the serial log (with SDA / SCL levels and, for a PCA9685 that answered, its registers) and the `i2c` object in [`GET /health`](../api.md#get-health). + +The OLED check is an address ACK only. A panel that loses its setup but still answers is not detected by itself; it is set up again whenever the PCA9685 recovers from a lost setup. + +The PCA9685 keeps its registers across an ESP32 reset; only its own power cycle clears them. That is why setup writes `MODE2` explicitly instead of trusting the power-on default. + +Bus start: the lines are cleared the same way, then `Wire.begin(I2C_SDA, I2C_SCL)` with no explicit frequency → Arduino-ESP32 default (typically **100 kHz**). Not set in this repo. PCA9685 **VCC** = **3V3**. OLED **VCC** = **3V3**. Pull-ups are on the main control board and on a typical OLED; the Adafruit PCA9685 breakout has its own. No extra pull-ups. diff --git a/docs/hardware/testing.md b/docs/hardware/testing.md index e820ebf1..98d8aa16 100644 --- a/docs/hardware/testing.md +++ b/docs/hardware/testing.md @@ -140,6 +140,7 @@ HTTP runs on STA when connected, or on setup AP at `192.168.4.1` during provisio | --- | --- | --- | | OLED `ERROR: OLED not found` then rest of boot runs | Nothing ACK’d at `0x3C` | OLED **VCC=3V3**, GND, SDA=GP0, **SCL**=GP1, common ground, address jumper still 0x3C | | OLED found but `ERROR: OLED initialization failed` | ACK then `display.begin` failed | Wiring/power glitch, wrong size module, I2C noise | +| OLED found but `ERROR: OLED setup failed; retrying after boot` | ACK, but a setup transfer failed | Retries by itself: an `[i2c] oled FAULT` and `recovered` line follow. Repeats point at wiring, power or I2C noise | | OLED shows `Join this WiFi` / AP name, then `Then open` / `192.168.4.1` | Setup AP mode active | Connect to the shown AP, open `http://192.168.4.1/config`, finish the servo + screen + LED + speaker + WiFi wizard | | OLED `WiFi failed` then setup AP | No saved network connected | Join setup AP, open `http://192.168.4.1/config`, enter home WiFi again | | `ERROR: PCA9685 not found` + red RGB + **hang** | Nothing ACK’d at `0x40` | PCA9685 **VCC=3V3** (not V+), GND, SDA/SCL, I2C address pads, +5V not required for the ACK but needed later for motion | @@ -147,8 +148,16 @@ HTTP runs on STA when connected, or on setup AP at `192.168.4.1` during provisio | I2S OK but `POST /test/audio` is silent | Amp or speaker | MAX98357A **Vin**=USB 5V, GND, GP2/3/4 → BCLK/LRC/DIN, speaker on **SPK+ / SPK-** (not on the PNG, not GND) | | Servos silent / twitch / ESP32 resets during `POST /test/movement` | Power or SIG | **V+** is +5V, SIG on ch 0–4, **common GND**, supply current — see [power.md](power.md) | | RGB never goes green | GPIO10 LED path | Board is C3-Zero (LED on GPIO10). Do not expect an external NeoPixel | - -OLED absence and Wi-Fi failure are **soft** fails. PCA9685 and I2S failures **halt** in `while (true)`. +| `[i2c] pca9685 FAULT #N` and `[i2c] oled FAULT #N` together, then `[i2c] bus restarted` and `recovered` lines | Both chips stopped answering after boot; firmware restarted the bus | `sda=LOW` / `scl=LOW` on the `bus restarted` line means a line was held: supply dip or wiring. Check supply current — see [power.md](power.md) | +| `[i2c] pca9685 FAULT #N … mode1=… mode2=… prescale=…` | PCA9685 answered but lost its setup (reset to defaults, or a corrupted write) | Recovers by itself without a bus restart. Repeats point at supply dips or I2C noise | +| `[i2c] pca9685 FAULT #N … write failed` | A servo write failed; servo writes and outputs stopped at once | Recovers by itself, usually by rewriting the channels only. Repeats point at supply dips or I2C noise | +| `[i2c] pca9685 FAULT #N … setup failed` | The PCA9685 setup did not read back correctly at boot; outputs stayed off | Recovers by itself once the checks start. Repeats point at supply dips or I2C noise | +| `[i2c] oled FAULT #N` alone | Only the OLED stopped answering | The servos are left alone. OLED **VCC**, GND, SDA, SCL, connector | +| `[i2c] still down … retry in … ms` keeps repeating | Bus or chip not coming back | Wiring, PCA9685 / OLED **VCC**, then a full power cycle | +| `[i2c] Wire.end failed` or `[i2c] Wire.begin failed` | The ESP32's I2C driver did not restart; both chips stay off until a later attempt succeeds | Report it with the serial log; a reset clears it | +| `i2c..faults` above `0` in `GET /health` | A fault happened since boot | `recoveries` equal to `faults` means all were recovered; `last_fault_ms` says when | + +OLED absence and Wi-Fi failure are **soft** fails. A PCA9685 that does not answer at boot, and an I2S failure, **halt** in `while (true)`. A PCA9685 that answers but whose setup does not read back is a fault instead (`setup failed`): boot continues with the servo outputs off, and the checks set it up again. I2C faults after boot do not halt: the firmware sets the faulted chip up again, restarting the bus when the PCA9685 stops answering ([interfaces.md](interfaces.md#i2c)). ## Power during the servo test diff --git a/platformio.ini b/platformio.ini index d2316dc6..728671bc 100644 --- a/platformio.ini +++ b/platformio.ini @@ -89,6 +89,7 @@ build_src_filter = + + + + + build_flags = -std=c++17 -Iinclude -Isrc ; Opt-in OLED-only application. Building does not upload or change robot settings. diff --git a/src/api/commands.cpp b/src/api/commands.cpp index 27a7d7df..19935ef5 100644 --- a/src/api/commands.cpp +++ b/src/api/commands.cpp @@ -7,6 +7,7 @@ #include "audio/audio.h" #include "display/oled.h" #include "extensions/push_button.h" +#include "hardware/i2c_health.h" #include "hardware/pca9685_servos.h" #include "hardware/rgb.h" #include "hardware/servo_wrapper.h" @@ -119,9 +120,22 @@ ApiResult apiMoveServo(int index, float angle) { } ApiResult apiParkServos() { + // A faulted servo driver would skip the move's writes, while the move still + // marks the centred pose for saving. Refuse before anything changes. + if (!i2cServoDriverUp()) { + return {false, 503, "servo driver down"}; + } + setAnimationImmediately(AnimationId::None); ensureAllServoOutputs(); centerAllServos(); + + // The driver faulted during the move: the joints never got to mid, so + // neither save that pose nor report a park. + if (!i2cServoDriverUp()) { + return {false, 503, "servo driver down"}; + } + const bool poseSaved = flushServoPoseToNvs(); setServoParkHoldUntilReboot(true); diff --git a/src/display/oled.h b/src/display/oled.h index 3e930873..72513b4c 100644 --- a/src/display/oled.h +++ b/src/display/oled.h @@ -34,6 +34,14 @@ void showTextThenIdle(const char* line1, const char* line2 = nullptr); void showIdleScreen(); void initOled(); +// True when the OLED answered at boot and its driver started, even if its +// setup then failed: the I2C checks watch it and retry that setup. +bool oledDetectedAtBoot(); +// Sets the OLED up again after an I2C fault and sends the last frame, so a +// static screen survives too. Checks every transfer; false leaves the OLED +// unavailable. With panelOn false the panel stays dark but holds the frame for +// when it wakes. Keeps the current rotation. +bool restartOled(bool panelOn); void runOledTest(); void showServoProgress( diff --git a/src/display/oled_init.cpp b/src/display/oled_init.cpp index 3901a4bb..ac1abf68 100644 --- a/src/display/oled_init.cpp +++ b/src/display/oled_init.cpp @@ -13,6 +13,95 @@ bool i2cDeviceConnected(uint8_t address) { return Wire.endTransmission() == 0; } +namespace { + +// The panel answered at boot and the driver has its frame buffer, whether or +// not the setup then went through. Separate from oledAvailable, which is +// whether it can be drawn on right now. +bool g_oledDetected = false; + +constexpr size_t kOledFrameBytes = OLED_WIDTH * ((OLED_HEIGHT + 7) / 8); +// Frame bytes per transfer, well inside Wire's buffer. +constexpr size_t kOledFrameChunk = 32; +// The driver runs its own transfers at 400 kHz and drops back to 100 kHz for +// the PCA9685 (its clkDuring / clkAfter defaults). The checked transfers here +// do the same, or a frame would take four times as long. +constexpr uint32_t kOledClockHz = 400000; +constexpr uint32_t kBusClockHz = 100000; + +// What begin() sends a 128x32 panel on its internal charge pump, minus the +// final DISPLAYON. The driver ignores failed writes, and a panel that missed +// part of this still answers its address, so this copy checks each transfer. +constexpr uint8_t kOledSetup[] = { + SSD1306_DISPLAYOFF, + SSD1306_SETDISPLAYCLOCKDIV, 0x80, + SSD1306_SETMULTIPLEX, OLED_HEIGHT - 1, + SSD1306_SETDISPLAYOFFSET, 0x00, + SSD1306_SETSTARTLINE | 0x00, + SSD1306_CHARGEPUMP, 0x14, + SSD1306_MEMORYMODE, 0x00, + SSD1306_SEGREMAP | 0x01, + SSD1306_COMSCANDEC, + SSD1306_SETCOMPINS, 0x02, + SSD1306_SETCONTRAST, 0x8F, + SSD1306_SETPRECHARGE, 0xF1, + SSD1306_SETVCOMDETECT, 0x40, + SSD1306_DISPLAYALLON_RESUME, + SSD1306_NORMALDISPLAY, + SSD1306_DEACTIVATE_SCROLL +}; +static_assert(OLED_WIDTH == 128 && OLED_HEIGHT == 32, "kOledSetup is for a 128x32 panel"); + +constexpr uint8_t kOledFrameWindow[] = { + SSD1306_PAGEADDR, 0x00, 0xFF, + SSD1306_COLUMNADDR, 0x00, OLED_WIDTH - 1 +}; + +// One I2C transfer: a control byte (0x00 commands, 0x40 frame data), then +// the bytes. True when the panel acknowledged all of it. +bool sendOled(uint8_t control, const uint8_t* bytes, size_t count) { + Wire.beginTransmission(OLED_ADDRESS); + Wire.write(control); + Wire.write(bytes, count); + return Wire.endTransmission() == 0; +} + +bool sendOledSetupAndFrame(bool panelOn) { + const uint8_t* frame = display.getBuffer(); + + if (frame == nullptr || + !sendOled(0x00, kOledSetup, sizeof(kOledSetup)) || + !sendOled(0x00, kOledFrameWindow, sizeof(kOledFrameWindow))) { + return false; + } + + for (size_t offset = 0; offset < kOledFrameBytes; offset += kOledFrameChunk) { + if (!sendOled(0x40, frame + offset, kOledFrameChunk)) { + return false; + } + } + + if (!panelOn) { + return true; + } + + const uint8_t on = SSD1306_DISPLAYON; + return sendOled(0x00, &on, 1); +} + +// Sends the panel its full setup and the frame buffer, then turns it on when +// panelOn. Safe to repeat: a panel that lost power comes back dark until it +// gets this again. The frame goes over while the panel is still off, so it +// wakes showing it. Leaves the frame buffer and the rotation as they are. +bool configureOled(bool panelOn) { + Wire.setClock(kOledClockHz); + const bool ok = sendOledSetupAndFrame(panelOn); + Wire.setClock(kBusClockHz); + return ok; +} + +} // namespace + void initOled() { serialLogPrintln(); serialLogPrintln("Checking OLED at 0x3C..."); @@ -25,21 +114,39 @@ void initOled() { serialLogPrintln("OLED found"); - if (!display.begin( - SSD1306_SWITCHCAPVCC, - OLED_ADDRESS - )) { + // begin() allocates and clears the frame buffer and sets the driver up; + // configureOled() then sends the setup again with each transfer checked. + if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) { serialLogPrintln("ERROR: OLED initialization failed"); oledAvailable = false; return; } + g_oledDetected = true; display.setRotation(settingsOledRotate180() ? 2 : 0); - display.clearDisplay(); - display.display(); + // A setup that did not go through leaves the panel detected but not + // available: the I2C checks then fault it and retry the setup. + if (!configureOled(true)) { + serialLogPrintln("ERROR: OLED setup failed; retrying after boot"); + oledAvailable = false; + return; + } oledAvailable = true; serialLogPrintln("OLED initialized"); } + +// Not begin(): it clears the frame buffer and ignores failed writes. The +// rotation is left alone too, since the setup wizard may be previewing one it +// has not saved. The frame buffer still holds the last frame, so a static +// screen comes back too. +bool oledDetectedAtBoot() { + return g_oledDetected; +} + +bool restartOled(bool panelOn) { + oledAvailable = i2cDeviceConnected(OLED_ADDRESS) && configureOled(panelOn); + return oledAvailable; +} diff --git a/src/hardware/i2c_fault.cpp b/src/hardware/i2c_fault.cpp new file mode 100644 index 00000000..82da542b --- /dev/null +++ b/src/hardware/i2c_fault.cpp @@ -0,0 +1,71 @@ +#include "hardware/i2c_fault.h" + +I2cFaultAction I2cFaultTracker::due(uint32_t now) const { + // Unsigned subtraction keeps the intervals right across the millis() wrap. + if (down_) { + return now - lastAttemptMs_ >= retryDelayMs_ + ? I2cFaultAction::Recover + : I2cFaultAction::None; + } + + return !checked_ || now - lastCheckMs_ >= kI2cCheckIntervalMs + ? I2cFaultAction::Check + : I2cFaultAction::None; +} + +bool I2cFaultTracker::noteCheck(bool ok, uint32_t now) { + checked_ = true; + lastCheckMs_ = now; + + if (ok) { + failures_ = 0; + return false; + } + + if (++failures_ < kI2cFailuresToFault) { + return false; + } + + takeDown(now); + return true; +} + +bool I2cFaultTracker::noteFault(uint32_t now) { + if (down_) { + return false; + } + + takeDown(now); + return true; +} + +void I2cFaultTracker::takeDown(uint32_t now) { + down_ = true; + faults_++; + lastFaultMs_ = now; + attempts_ = 0; + // The first recovery attempt runs right away. + retryDelayMs_ = 0; + lastAttemptMs_ = now; +} + +void I2cFaultTracker::noteRecovery(bool ok, uint32_t now) { + attempts_++; + lastAttemptMs_ = now; + + if (ok) { + down_ = false; + failures_ = 0; + recoveries_++; + lastCheckMs_ = now; + return; + } + + if (retryDelayMs_ == 0) { + retryDelayMs_ = kI2cFirstRetryMs; + } else if (retryDelayMs_ >= kI2cMaxRetryMs / 2) { + retryDelayMs_ = kI2cMaxRetryMs; + } else { + retryDelayMs_ *= 2; + } +} diff --git a/src/hardware/i2c_fault.h b/src/hardware/i2c_fault.h new file mode 100644 index 00000000..31d9e9a4 --- /dev/null +++ b/src/hardware/i2c_fault.h @@ -0,0 +1,56 @@ +#pragma once + +#include + +constexpr uint32_t kI2cCheckIntervalMs = 500; +// One failed check can be a glitch; two in a row take the bus down. +constexpr uint8_t kI2cFailuresToFault = 2; +constexpr uint32_t kI2cFirstRetryMs = 1000; +constexpr uint32_t kI2cMaxRetryMs = 30000; + +enum class I2cFaultAction : uint8_t { + None, + Check, // bus is up: probe the devices + Recover // bus is down: try to bring it back +}; + +// Decides when the I2C bus counts as down and when to try bringing it back. +// Pure logic: the caller does the bus work and reports each result. +class I2cFaultTracker { +public: + I2cFaultAction due(uint32_t now) const; + + // Result of a health check while up. True when this check took the bus down. + bool noteCheck(bool ok, uint32_t now); + + // A failure that counts as a fault on its own, such as a failed write: the + // bus goes down at once. True when this took it down; false when it was down. + bool noteFault(uint32_t now); + + // Result of a recovery attempt while down. A failed attempt doubles the wait + // before the next one, up to kI2cMaxRetryMs. + void noteRecovery(bool ok, uint32_t now); + + bool down() const { return down_; } + uint32_t faults() const { return faults_; } + uint32_t recoveries() const { return recoveries_; } + // millis() of the latest fault; 0 when there has been none. + uint32_t lastFaultMs() const { return lastFaultMs_; } + // Recovery attempts since the current fault began. + uint32_t attempts() const { return attempts_; } + uint32_t retryDelayMs() const { return retryDelayMs_; } + +private: + void takeDown(uint32_t now); + + bool down_ = false; + bool checked_ = false; + uint8_t failures_ = 0; + uint32_t lastCheckMs_ = 0; + uint32_t lastAttemptMs_ = 0; + uint32_t retryDelayMs_ = 0; + uint32_t attempts_ = 0; + uint32_t faults_ = 0; + uint32_t recoveries_ = 0; + uint32_t lastFaultMs_ = 0; +}; diff --git a/src/hardware/i2c_health.cpp b/src/hardware/i2c_health.cpp new file mode 100644 index 00000000..874c2d10 --- /dev/null +++ b/src/hardware/i2c_health.cpp @@ -0,0 +1,454 @@ +#include +#include + +#include "display/oled.h" +#include "hardware/i2c_fault.h" +#include "hardware/i2c_health.h" +#include "hardware/servo_wrapper.h" +#include "pins.h" +#include "serial_log.h" +#include "sleep.h" + +namespace { + +constexpr uint8_t kBusClearPulses = 9; +constexpr uint32_t kBusClearHalfPeriodUs = 5; +// Wire's default is 50 ms. Every transfer here finishes in a few: the +// largest, a 128-byte OLED frame chunk, takes about 3 ms at 400 kHz and 12 ms +// at 100 kHz. On a dead bus each transfer waits this long, and a tick does at +// most one device's work, about one timeout. The speaker's DMA buffer holds +// about 65 ms (6 x 240 frames at 22,050 Hz), so a tick never starves it. +constexpr uint16_t kI2cTransferTimeoutMs = 20; + +enum class RecoveryResult : uint8_t { + InProgress, // a step ran; call again next tick + Succeeded, + Failed +}; + +struct BusLines { + bool sdaHigh; + bool sclHigh; +}; + +// One watched chip. Each has its own tracker, so a chip that stays down does +// not stop the checks on the other, and a fault on one does not restart both. +struct Device { + const char* name; + bool (*healthy)(); + void (*stopWrites)(); + // One step of a recovery, bounded by about one transfer timeout. + RecoveryResult (*recover)(uint32_t now); + // Extra text for the fault log line; null when there is none. + void (*logFaultDetail)(); + I2cFaultTracker tracker; +}; + +bool servoDriverHealthy(); +RecoveryResult recoverServoDriver(uint32_t now); +void logServoDriverDetail(); +bool oledHealthy(); +void stopOledWrites(); +RecoveryResult recoverOled(uint32_t now); + +Device g_servoDriver = { + // At a fault the outputs go off, so a corrupted setup cannot keep driving + // the servos. Recovery turns them back on once the setup reads back right. + "pca9685", servoDriverHealthy, disableServoOutputs, recoverServoDriver, logServoDriverDetail, {} +}; +Device g_oled = {"oled", oledHealthy, stopOledWrites, recoverOled, nullptr, {}}; + +bool g_started = false; +// The robot runs without an OLED; only watch one that was there at boot, +// including one whose setup failed there, so that setup gets retried. +bool g_oledExpected = false; +// False after a failed Wire restart. Wire must not be used then: without its +// buffers a transfer leaks Wire's lock, and the next restart would wait on +// that lock forever. +bool g_wireUp = true; +// Set when the PCA9685 is found without its setup; cleared once the OLED has +// been set up again. Kept across failed attempts: a retry finds the PCA9685's +// setup restored and would otherwise skip the panel. +bool g_oledRestartPending = false; +// Where the PCA9685's recovery is. It runs one step per tick, each a short +// burst of transfers, so a recovery never holds the loop for long. +enum class ServoRecoveryStep : uint8_t { + Probe, // does the chip answer? + RestartBus, // it did not: restart Wire, then ask again + CheckSetup, // does it still hold its setup? + SetUp, // it did not: set it up again + ChannelsOff, // every channel off, outputs off + Settle, // wait for the off values to take effect + Oled, // set the panel up again after a power loss + ChannelsOn // outputs on, then every channel written; ends the recovery +}; +ServoRecoveryStep g_servoStep = ServoRecoveryStep::Probe; +uint32_t g_channelsOffMs = 0; +// Registers from the latest PCA9685 check, for the fault log line. +ServoPwmDriverState g_servoDriverState = {}; + +// A device interrupted mid-byte keeps driving SDA until it has been clocked +// through the rest of that byte. Clock it out, send a STOP, and report where +// the lines settled: a line still low means something is holding the bus. +// All nine clocks, even once SDA reads high: in an interrupted read a high +// SDA can be a 1 data bit, and the device would pull the line low again under +// the STOP. Nine clocks always pass an acknowledge slot, where the released +// SDA reads as a NACK and ends the read. +BusLines clearBus() { + pinMode(I2C_SDA, INPUT_PULLUP); + pinMode(I2C_SCL, OUTPUT_OPEN_DRAIN); + digitalWrite(I2C_SCL, HIGH); + delayMicroseconds(kBusClearHalfPeriodUs); + + for (uint8_t i = 0; i < kBusClearPulses; i++) { + digitalWrite(I2C_SCL, LOW); + delayMicroseconds(kBusClearHalfPeriodUs); + digitalWrite(I2C_SCL, HIGH); + delayMicroseconds(kBusClearHalfPeriodUs); + } + + // STOP: SDA rises while SCL is high. + digitalWrite(I2C_SCL, LOW); + delayMicroseconds(kBusClearHalfPeriodUs); + pinMode(I2C_SDA, OUTPUT_OPEN_DRAIN); + digitalWrite(I2C_SDA, LOW); + delayMicroseconds(kBusClearHalfPeriodUs); + digitalWrite(I2C_SCL, HIGH); + delayMicroseconds(kBusClearHalfPeriodUs); + digitalWrite(I2C_SDA, HIGH); + delayMicroseconds(kBusClearHalfPeriodUs); + + pinMode(I2C_SDA, INPUT_PULLUP); + pinMode(I2C_SCL, INPUT_PULLUP); + delayMicroseconds(kBusClearHalfPeriodUs); + + return {digitalRead(I2C_SDA) == HIGH, digitalRead(I2C_SCL) == HIGH}; +} + +void logLines(const BusLines& lines) { + serialLogPrint(" sda="); + serialLogPrint(lines.sdaHigh ? "high" : "LOW"); + serialLogPrint(" scl="); + serialLogPrintln(lines.sclHigh ? "high" : "LOW"); +} + +// `cause` names a fault found outside a check; a check's fault logs the +// device's own detail instead. +void logFault(const Device& device, uint32_t now, const char* cause = nullptr) { + serialLogPrint("[i2c] "); + serialLogPrint(device.name); + serialLogPrint(" FAULT #"); + serialLogPrint(device.tracker.faults()); + serialLogPrint(" at "); + serialLogPrint(now); + serialLogPrint(" ms"); + + if (cause != nullptr) { + serialLogPrint(" "); + serialLogPrint(cause); + } else if (device.logFaultDetail != nullptr) { + device.logFaultDetail(); + } + + serialLogPrintln(); +} + +void noteRecovery(Device& device, bool ok) { + const uint32_t now = millis(); + device.tracker.noteRecovery(ok, now); + + serialLogPrint("[i2c] "); + serialLogPrint(device.name); + + if (ok) { + serialLogPrint(" recovered, attempt "); + serialLogPrint(device.tracker.attempts()); + serialLogPrint(", down "); + serialLogPrint(now - device.tracker.lastFaultMs()); + serialLogPrintln(" ms"); + return; + } + + serialLogPrint(" still down, attempt "); + serialLogPrint(device.tracker.attempts()); + serialLogPrint(", retry in "); + serialLogPrint(device.tracker.retryDelayMs()); + serialLogPrintln(" ms"); +} + +// Restarts Wire with the lines freed. False when Wire did not come back or a +// line is still held: every transfer would fail, so the caller skips the +// probes and their timeouts. Only the PCA9685's recovery calls this, so servo +// writes are already off. +bool restartBus() { + g_wireUp = false; + + if (!Wire.end()) { + // Wire dropped its buffers but kept its driver. begin() would report + // success without them, so leave it down and end() again next attempt. + stopOledWrites(); + serialLogPrintln("[i2c] Wire.end failed"); + return false; + } + + const BusLines lines = clearBus(); + g_wireUp = Wire.begin(I2C_SDA, I2C_SCL); + const bool usable = g_wireUp && lines.sdaHigh && lines.sclHigh; + + // A held line fails every transfer as surely as a dead Wire: stop the + // OLED's frames, and let its own checks recover it once the bus is back. + if (!usable) { + stopOledWrites(); + } + + serialLogPrint(g_wireUp ? "[i2c] bus restarted" : "[i2c] Wire.begin failed"); + logLines(lines); + + return usable; +} + +// The restart puts the last frame back on the panel; a sleeping robot keeps +// it dark. +bool restartOledForSleepState() { + return restartOled(!isSleeping()); +} + +// Both chips share a supply, so a PCA9685 that lost its setup is reason to set +// the panel up again too, even when it still answers. A failure here while the +// OLED counts as up clears oledAvailable, and the OLED's own checks then take +// it down and recover it. +void restartOledAfterPowerLoss() { + g_oledRestartPending = false; + + if (!g_oledExpected) { + return; + } + + const bool ok = restartOledForSleepState(); + + if (g_oled.tracker.down()) { + noteRecovery(g_oled, ok); + } +} + +bool servoDriverHealthy() { + g_servoDriverState = {}; + + if (!g_wireUp) { + return false; + } + + return servoPwmDriverState(g_servoDriverState); +} + +// Why a check faulted the PCA9685: the registers it answered with. +void logServoDriverDetail() { + if (!g_servoDriverState.answered) { + return; + } + + serialLogPrint(" mode1=0x"); + serialLogPrint(g_servoDriverState.mode1, HEX); + serialLogPrint(" mode2=0x"); + serialLogPrint(g_servoDriverState.mode2, HEX); + serialLogPrint(" prescale=0x"); + serialLogPrint(g_servoDriverState.prescale, HEX); +} + +bool servoDriverAnswers() { + return g_wireUp && i2cDeviceConnected(PCA9685_ADDRESS); +} + +RecoveryResult endServoRecovery(RecoveryResult result) { + g_servoStep = ServoRecoveryStep::Probe; + return result; +} + +RecoveryResult nextServoStep(ServoRecoveryStep step) { + g_servoStep = step; + return RecoveryResult::InProgress; +} + +// The PCA9685 is the required chip, so its recovery is the one that restarts +// the bus. A chip that still answers proves the bus works. One that also kept +// its setup only lost a write or a read: rewriting the channels is enough, and +// the healthy OLED is left alone. A lost setup means the chip lost power or +// was corrupted: it is set up from scratch, and so is the panel. +RecoveryResult recoverServoDriver(uint32_t now) { + switch (g_servoStep) { + case ServoRecoveryStep::Probe: + return nextServoStep( + servoDriverAnswers() ? ServoRecoveryStep::CheckSetup : ServoRecoveryStep::RestartBus + ); + + case ServoRecoveryStep::RestartBus: + if (!restartBus() || !servoDriverAnswers()) { + return endServoRecovery(RecoveryResult::Failed); + } + return nextServoStep(ServoRecoveryStep::CheckSetup); + + case ServoRecoveryStep::CheckSetup: + if (servoPwmDriverState(g_servoDriverState)) { + return nextServoStep(ServoRecoveryStep::ChannelsOff); + } + g_oledRestartPending = true; + return nextServoStep(ServoRecoveryStep::SetUp); + + case ServoRecoveryStep::SetUp: + if (!setUpServoPwmDriver()) { + return endServoRecovery(RecoveryResult::Failed); + } + return nextServoStep(ServoRecoveryStep::ChannelsOff); + + case ServoRecoveryStep::ChannelsOff: + if (!writeServoChannelsOff()) { + return endServoRecovery(RecoveryResult::Failed); + } + // After the writes, not `now`: they must not eat into the settle time. + g_channelsOffMs = millis(); + return nextServoStep(ServoRecoveryStep::Settle); + + case ServoRecoveryStep::Settle: + if (now - g_channelsOffMs < kServoOutputSettleMs) { + return RecoveryResult::InProgress; + } + return nextServoStep( + g_oledRestartPending ? ServoRecoveryStep::Oled : ServoRecoveryStep::ChannelsOn + ); + + case ServoRecoveryStep::Oled: + restartOledAfterPowerLoss(); + return nextServoStep(ServoRecoveryStep::ChannelsOn); + + // Last, so the channels written here are the latest the firmware asked + // for: the chip counts as up again in the same pass. A step after it + // would leave a pass where servo writes are still skipped but cached. + case ServoRecoveryStep::ChannelsOn: + if (!enableServoOutputsWithChannels()) { + return endServoRecovery(RecoveryResult::Failed); + } + return endServoRecovery(RecoveryResult::Succeeded); + } + + return endServoRecovery(RecoveryResult::Failed); +} + +// oledAvailable is part of the check: a restart that failed outside an OLED +// fault cleared it, and only a fault and its recovery set it again. +bool oledHealthy() { + return g_wireUp && oledAvailable && i2cDeviceConnected(OLED_ADDRESS); +} + +void stopOledWrites() { + oledAvailable = false; +} + +// Never restarts the bus: an OLED fault alone must not disturb the servos. +// A dead bus is left to the PCA9685's recovery. +RecoveryResult recoverOled(uint32_t) { + return g_wireUp && restartOledForSleepState() + ? RecoveryResult::Succeeded + : RecoveryResult::Failed; +} + +// The device is down: stop its writes, since each failed one blocks the loop +// for its timeout. +void enterFault(Device& device, uint32_t now, const char* cause = nullptr) { + device.stopWrites(); + logFault(device, now, cause); +} + +// True when it did bus work: a check, or a step of a recovery. +bool service(Device& device) { + // Read here, not once per tick: the other chip's work can stamp this + // tracker with a later time. + const uint32_t now = millis(); + + switch (device.tracker.due(now)) { + case I2cFaultAction::Check: + if (device.tracker.noteCheck(device.healthy(), now)) { + enterFault(device, now); + } + return true; + case I2cFaultAction::Recover: { + // A recovery in progress keeps its tracker due until it ends. + const RecoveryResult result = device.recover(now); + + if (result != RecoveryResult::InProgress) { + noteRecovery(device, result == RecoveryResult::Succeeded); + } + return true; + } + case I2cFaultAction::None: + return false; + } + + return false; +} + +I2cDeviceHealth deviceHealth(const Device& device, bool up) { + return { + up, + device.tracker.faults(), + device.tracker.recoveries(), + device.tracker.lastFaultMs() + }; +} + +} // namespace + +void initI2cBus() { + const BusLines lines = clearBus(); + + if (!lines.sdaHigh || !lines.sclHigh) { + serialLogPrint("[i2c] bus held at boot:"); + logLines(lines); + } + + // Kept across Wire restarts: it is a member, set once. + Wire.setTimeOut(kI2cTransferTimeoutMs); + g_wireUp = Wire.begin(I2C_SDA, I2C_SCL); + + if (!g_wireUp) { + serialLogPrintln("[i2c] Wire.begin failed at boot"); + } +} + +void beginI2cHealth() { + g_oledExpected = oledDetectedAtBoot(); + g_started = true; +} + +void updateI2cHealth() { + if (!g_started) { + return; + } + + // At most one chip's work per tick, so a tick costs about one transfer + // timeout at worst. The OLED's turn comes a tick later. + if (!service(g_servoDriver) && g_oledExpected) { + service(g_oled); + } +} + +bool i2cServoDriverUp() { + return !g_servoDriver.tracker.down(); +} + +void i2cNoteServoDriverFault(const char* cause) { + const uint32_t now = millis(); + + if (g_servoDriver.tracker.noteFault(now)) { + enterFault(g_servoDriver, now, cause); + } +} + +I2cHealth i2cHealth() { + const bool servoDriverUp = i2cServoDriverUp(); + + return { + servoDriverUp && (!g_oledExpected || oledAvailable), + deviceHealth(g_servoDriver, servoDriverUp), + deviceHealth(g_oled, oledAvailable) + }; +} diff --git a/src/hardware/i2c_health.h b/src/hardware/i2c_health.h new file mode 100644 index 00000000..32096d25 --- /dev/null +++ b/src/hardware/i2c_health.h @@ -0,0 +1,41 @@ +#pragma once + +#include + +// Watches the I2C bus the PCA9685 and the OLED share. The SSD1306 driver +// ignores failed writes and the firmware used to ignore the PCA9685 driver's +// write status, so a dead bus left the robot frozen while every write waited +// out its timeout and starved audio and HTTP. + +// Frees a bus a device still holds from before a reset, then starts Wire. +void initI2cBus(); + +// Starts the checks. Call once the PCA9685 and the OLED are set up. +void beginI2cHealth(); + +// Checks each chip while it is up and sets it up again while it is down. +// Cheap enough for every tick. +void updateI2cHealth(); + +// False while the PCA9685 cannot be used: skip servo writes. +bool i2cServoDriverUp(); + +// The PCA9685 failed outside a check, such as a failed write or a boot setup +// that did not take. It goes down at once: its outputs go off and servo writes +// stop until the recovery has set it up again. `cause` goes in the fault log. +void i2cNoteServoDriverFault(const char* cause); + +struct I2cDeviceHealth { + bool ok; + uint32_t faults; + uint32_t recoveries; + uint32_t lastFaultMs; +}; + +struct I2cHealth { + bool ok; + I2cDeviceHealth servoDriver; + I2cDeviceHealth oled; +}; + +I2cHealth i2cHealth(); diff --git a/src/hardware/servo_wrapper.cpp b/src/hardware/servo_wrapper.cpp index ebe5679e..643124ae 100644 --- a/src/hardware/servo_wrapper.cpp +++ b/src/hardware/servo_wrapper.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -7,6 +8,7 @@ #include "animation/util.h" #include "pins.h" #include "servos.h" +#include "hardware/i2c_health.h" #include "hardware/servo_wrapper.h" #include "serial_log.h" #include "settings/settings.h" @@ -19,6 +21,25 @@ constexpr const char* kPoseNs = "te"; constexpr const char* kPoseKey = "spose"; constexpr uint32_t kPoseSaveDebounceMs = 500; +constexpr uint16_t kServoPwmHz = 50; + +constexpr uint8_t kPcaMode1Reg = 0x00; +constexpr uint8_t kPcaMode2Reg = 0x01; +// Awake, on the internal clock, with register auto-increment on: setPWM +// writes four registers in one transfer and needs it. EXTCLK is sticky (only a +// power cycle clears it), so a corrupted one keeps the chip down, visibly. +constexpr uint8_t kPcaMode1Mask = MODE1_SLEEP | MODE1_AI | MODE1_EXTCLK; +constexpr uint8_t kPcaMode1Servo = MODE1_AI; +// Totem-pole outputs, not inverted: what a servo signal line needs. Any other +// value leaves the servos without a usable pulse while every write still +// succeeds. +constexpr uint8_t kPcaMode2Servo = MODE2_OUTDRV; +// What setPWMFreq(kServoPwmHz) programs, by the Adafruit driver's own formula. +// Computed, not read back: a readback after a lost write would bless the +// wrong value. A chip that lost power comes back with a different default. +constexpr uint8_t kPcaPrescaleServo = + static_cast(FREQUENCY_OSCILLATOR / (kServoPwmHz * 4096.0) + 0.5 - 1); + static bool g_allOutputsReleased = false; static bool g_parkHoldUntilReboot = false; static bool g_poseDirty = false; @@ -238,11 +259,165 @@ void setServoParkHoldUntilReboot(bool hold) { g_parkHoldUntilReboot = hold; } +namespace { + +// Skipped while the PCA9685 is unusable, so a dead bus does not stall the +// loop. The first failed write takes the chip down at once, with its outputs +// off, even inside a blocking move that never reaches the health check; the +// recovery rewrites every channel once the chip is back. +void writeChannel(uint8_t channel, uint16_t off) { + if (!i2cServoDriverUp()) { + return; + } + + if (pwm.setPWM(channel, 0, off) != 0) { + i2cNoteServoDriverFault("write failed"); + } +} + +// Reads `count` registers from `reg` in one transfer; relies on auto-increment. +bool readPcaRegisters(uint8_t reg, uint8_t* values, uint8_t count) { + Wire.beginTransmission(PCA9685_ADDRESS); + Wire.write(reg); + // Without a STOP nothing is sent yet; a failure shows up in requestFrom. + Wire.endTransmission(false); + + if (Wire.requestFrom(PCA9685_ADDRESS, count) != count) { + return false; + } + + for (uint8_t i = 0; i < count; i++) { + values[i] = static_cast(Wire.read()); + } + + return true; +} + +// Writes one channel and reads it back: an acknowledged write can still land +// corrupted on a bus that just glitched. +bool writeChannelVerified(uint8_t channel, uint16_t off) { + if (pwm.setPWM(channel, 0, off) != 0) { + return false; + } + + uint8_t got[4] = {}; + const uint8_t want[4] = {0, 0, static_cast(off & 0xFF), static_cast(off >> 8)}; + + return readPcaRegisters(PCA9685_LED0_ON_L + 4 * channel, got, 4) && + memcmp(got, want, sizeof(want)) == 0; +} + +// Output mode and servo frequency, shared by boot and recovery. The chip must +// be awake first: setPWMFreq() restores the sleep bit it finds. False when the +// MODE2 write failed: the frequency is then skipped, since on a dead bus each +// of its transfers would wait out a timeout too. +bool writeServoSetup() { + // The Adafruit driver never writes MODE2, so a value corrupted by a bus + // glitch would outlive every restart. The chip keeps its registers across + // an ESP32 reset: only its own power cycle clears them. + Wire.beginTransmission(PCA9685_ADDRESS); + Wire.write(kPcaMode2Reg); + Wire.write(kPcaMode2Servo); + + if (Wire.endTransmission() != 0) { + return false; + } + + pwm.setPWMFreq(kServoPwmHz); + return true; +} + +// The end of boot: the recovery's sequence in one go. Outputs stay off unless +// the setup took: a wrong output mode or frequency must not reach the servos. +bool finishServoPwmDriverSetup() { + ServoPwmDriverState state; + + if (!servoPwmDriverState(state) || !writeServoChannelsOff()) { + return false; + } + + delay(kServoOutputSettleMs); + return enableServoOutputsWithChannels(); +} + +} // namespace + +bool servoPwmDriverState(ServoPwmDriverState& state) { + state = {}; + + // MODE1 and MODE2 are adjacent, so one read returns both. With + // auto-increment off the second byte repeats MODE1 and fails the MODE2 check. + uint8_t modes[2] = {}; + state.answered = readPcaRegisters(kPcaMode1Reg, modes, 2); + + if (!state.answered) { + return false; + } + + state.mode1 = modes[0]; + state.mode2 = modes[1]; + state.prescale = pwm.readPrescale(); + + return (state.mode1 & kPcaMode1Mask) == kPcaMode1Servo && + state.mode2 == kPcaMode2Servo && + state.prescale == kPcaPrescaleServo; +} + +bool setUpServoPwmDriver() { + disableServoOutputs(); + + // Not pwm.begin(): it programs 1 kHz before writeServoSetup() sets 50 Hz, + // and on a board with OE not wired the live servos would get those pulses. + // reset() wakes the chip, which is all begin() adds here. + pwm.reset(); + + if (!writeServoSetup()) { + return false; + } + + ServoPwmDriverState state; + return servoPwmDriverState(state); +} + +// Turning OE on is not synchronised to the PWM period: done while a channel is +// mid-pulse it would cut that pulse short, and the servo would read a wrong +// angle. Channel writes are: the PCA9685 applies new values at the end of a +// LOW cycle (datasheet 7.3.3). So the outputs come on with every channel off, +// and the channels are written after, each starting on a whole pulse. +bool writeServoChannelsOff() { + disableServoOutputs(); + + // Stops at the first failure: on a dead bus each further write waits out + // its timeout, and the next attempt writes every channel anyway. + for (int i = 0; i < SERVO_COUNT; i++) { + if (!writeChannelVerified(SERVO_SPECS[i].channel, kPca9685FullOff)) { + return false; + } + } + + return true; +} + +bool enableServoOutputsWithChannels() { + enableServoOutputs(); + + // The servos kept animating in software while the chip was unreachable; + // this puts every channel where the firmware thinks it is. + for (int i = 0; i < SERVO_COUNT; i++) { + if (!g_servos[i].resyncOutput()) { + disableServoOutputs(); + return false; + } + } + + return true; +} + void initServoPwmDriver() { disableServoOutputs(); pwm.begin(); - pwm.setPWMFreq(50); + const bool setupWritten = writeServoSetup(); delay(100); uint8_t pose[SERVO_COUNT]; @@ -255,15 +430,18 @@ void initServoPwmDriver() { } for (int i = 0; i < SERVO_COUNT; i++) { - g_servos[i].setPosition(static_cast(pose[i])); + g_servos[i].restorePosition(static_cast(pose[i])); } memcpy(g_lastSavedPose, pose, SERVO_COUNT); g_lastSavedPoseValid = true; g_poseDirty = false; - delay(10); - enableServoOutputs(); + // Same path as a restart, so boot and recovery cannot drift apart. A setup + // that did not take leaves the outputs off and the recovery to the checks. + if (!setupWritten || !finishServoPwmDriverSetup()) { + i2cNoteServoDriverFault("setup failed"); + } } ServoWrapper& servoAt(int index) { @@ -300,6 +478,14 @@ void ServoWrapper::setPosition(float angle) { lastUpdateMs_ = 0; } +void ServoWrapper::restorePosition(float angle) { + angle_ = clampServoAngle(index_, angle); + target_ = angle_; + lastUpdateMs_ = 0; + // Not written yet: the next write for this servo must go out. + lastPulse_ = UINT16_MAX; +} + void ServoWrapper::setNormTarget(float n, float speedDegS) { setTarget(servoNormToDeg(index_, n), speedDegS); } @@ -403,11 +589,7 @@ void ServoWrapper::ensureServoOutput() { lastPulse_ = UINT16_MAX; const uint16_t pulse = angleToPulse(angle_); - pwm.setPWM( - SERVO_SPECS[index_].channel, - 0, - pulse - ); + writeChannel(SERVO_SPECS[index_].channel, pulse); lastPulse_ = pulse; serialLogPrint("[servo] pwm on "); @@ -421,15 +603,31 @@ void ServoWrapper::releaseServoOutput() { return; } - pwm.setPWM( - SERVO_SPECS[index_].channel, - 0, - kPca9685FullOff - ); + writeChannel(SERVO_SPECS[index_].channel, kPca9685FullOff); lastPulse_ = UINT16_MAX; released_ = true; } +bool ServoWrapper::resyncOutput() { + // Written straight to the chip, past writeChannel()'s check: this runs at + // boot and while the chip still counts as down. + if (released_) { + lastPulse_ = UINT16_MAX; + return writeChannelVerified(SERVO_SPECS[index_].channel, kPca9685FullOff); + } + + const uint16_t pulse = angleToPulse(angle_); + + if (!writeChannelVerified(SERVO_SPECS[index_].channel, pulse)) { + // Not cached: the next write for this servo must go out. + lastPulse_ = UINT16_MAX; + return false; + } + + lastPulse_ = pulse; + return true; +} + void ServoWrapper::writeAngle(float angle, bool log, bool electrical) { ensureServoOutput(); angle = clampAngle(index_, angle, electrical); @@ -446,11 +644,7 @@ void ServoWrapper::writeAngle(float angle, bool log, bool electrical) { } if (pulse != lastPulse_) { - pwm.setPWM( - SERVO_SPECS[index_].channel, - 0, - pulse - ); + writeChannel(SERVO_SPECS[index_].channel, pulse); lastPulse_ = pulse; } diff --git a/src/hardware/servo_wrapper.h b/src/hardware/servo_wrapper.h index 3f3f5dd0..860d50f2 100644 --- a/src/hardware/servo_wrapper.h +++ b/src/hardware/servo_wrapper.h @@ -19,6 +19,9 @@ class ServoWrapper { float speedDegS = SERVO_MAX_SPEED_DEG_S ); void setPosition(float angle); + // Sets the firmware's angle without writing the chip; boot writes every + // channel afterwards. + void restorePosition(float angle); void setNormTarget( float n, float speedDegS = SERVO_MAX_SPEED_DEG_S @@ -29,6 +32,9 @@ class ServoWrapper { bool isMoving() const; void ensureServoOutput(); void releaseServoOutput(); + // Rewrites this channel from the firmware's state after a driver restart. + // False when the write failed or did not read back. + bool resyncOutput(); friend void servoMoveAllSmoothTo( const float targets[SERVO_COUNT], @@ -56,6 +62,30 @@ void initServoOutputPin(); void disableServoOutputs(); void enableServoOutputs(); void initServoPwmDriver(); +struct ServoPwmDriverState { + bool answered; + uint8_t mode1; + uint8_t mode2; + uint8_t prescale; +}; + +// Reads the PCA9685's setup registers. True when it answers, is awake with +// auto-increment on, and holds the servo output mode and prescale. +bool servoPwmDriverState(ServoPwmDriverState& state); +// Bringing the outputs back, in steps a caller can spread over several loop +// passes: setUpServoPwmDriver() if the setup was lost, writeServoChannelsOff(), +// at least kServoOutputSettleMs, then enableServoOutputsWithChannels(). Each +// returns false, with the outputs off, when a write failed or did not read +// back. +constexpr uint32_t kServoOutputSettleMs = 25; // more than one 20 ms period +// Wakes the PCA9685 and writes its output mode and frequency, outputs off. +// True when the setup reads back. +bool setUpServoPwmDriver(); +// Turns every servo channel fully off, outputs off. +bool writeServoChannelsOff(); +// Turns the outputs on, then writes every channel from the firmware's state; +// keeps the current pose. +bool enableServoOutputsWithChannels(); void servoMoveAllSmoothTo( const float targets[SERVO_COUNT], float speedDegS = SERVO_BOOT_SPEED_DEG_S diff --git a/src/http/health_handlers.cpp b/src/http/health_handlers.cpp index 583e942b..803b2ba4 100644 --- a/src/http/health_handlers.cpp +++ b/src/http/health_handlers.cpp @@ -4,11 +4,23 @@ #include "display/oled.h" #include "firmware_version.h" #include "hardware/chip_temp.h" +#include "hardware/i2c_health.h" #include "http/handlers.h" #include "http/response.h" #include "network/wifi_connect.h" #include "settings/settings.h" +namespace { + +void writeI2cDevice(JsonObject out, const I2cDeviceHealth& device) { + out["ok"] = device.ok; + out["faults"] = device.faults; + out["recoveries"] = device.recoveries; + out["last_fault_ms"] = device.lastFaultMs; +} + +} // namespace + void handleAuth(WebServer& server) { JsonDocument body; body["ok"] = true; @@ -51,5 +63,11 @@ void handleHealth(WebServer& server) { body["setup_ap_ssid"] = provisioning ? wifiApSsid() : ""; body["setup_ap_ip"] = provisioning ? wifiApIpText() : ""; body["oled"] = oledAvailable; + + const I2cHealth bus = i2cHealth(); + JsonObject i2c = body["i2c"].to(); + i2c["ok"] = bus.ok; + writeI2cDevice(i2c["pca9685"].to(), bus.servoDriver); + writeI2cDevice(i2c["oled"].to(), bus.oled); sendJson(server, 200, body); } diff --git a/src/main.cpp b/src/main.cpp index 6f844498..3e188bca 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,6 +6,7 @@ #include "firmware_version.h" #include "hardware/rgb.h" #include "hardware/chip_temp.h" +#include "hardware/i2c_health.h" #include "display/oled.h" #include "audio/audio.h" #include "animation.h" @@ -60,10 +61,7 @@ void setup() { serialLogPrintln("SDA = GP0"); serialLogPrintln("SCL = GP1"); - Wire.begin( - I2C_SDA, - I2C_SCL - ); + initI2cBus(); delay(100); @@ -71,6 +69,8 @@ void setup() { initOled(); + beginI2cHealth(); + const bool sleepServos = bootSleepInertiaUsesServos(); if (bootLoadingIsProgress()) { diff --git a/src/robot_tick.cpp b/src/robot_tick.cpp index c8c68021..0e743292 100644 --- a/src/robot_tick.cpp +++ b/src/robot_tick.cpp @@ -5,6 +5,7 @@ #include "animation.h" #include "audio/wav_stream.h" #include "display/oled.h" +#include "hardware/i2c_health.h" #include "hardware/rgb.h" #include "hardware/servo_wrapper.h" #include "network/wifi_connect.h" @@ -12,6 +13,7 @@ void tickRobot() { const uint32_t now = millis(); + updateI2cHealth(); pollWifi(); updateProvisioningOled(now); updateAnimation(); diff --git a/test/test_i2c_fault/test_i2c_fault.cpp b/test/test_i2c_fault/test_i2c_fault.cpp new file mode 100644 index 00000000..e8c2be61 --- /dev/null +++ b/test/test_i2c_fault/test_i2c_fault.cpp @@ -0,0 +1,176 @@ +#include + +#include "hardware/i2c_fault.h" + +namespace { + +void expectAction(I2cFaultAction want, I2cFaultAction got) { + TEST_ASSERT_EQUAL(static_cast(want), static_cast(got)); +} + +// Fails enough checks in a row to take the bus down; returns the time of the last. +uint32_t takeDown(I2cFaultTracker& tracker, uint32_t start) { + uint32_t now = start; + for (uint8_t i = 0; i < kI2cFailuresToFault; i++) { + now = start + i * kI2cCheckIntervalMs; + tracker.noteCheck(false, now); + } + return now; +} + +} // namespace + +void test_checks_on_first_call_then_on_interval() { + I2cFaultTracker tracker; + expectAction(I2cFaultAction::Check, tracker.due(0)); + + tracker.noteCheck(true, 100); + expectAction(I2cFaultAction::None, tracker.due(100 + kI2cCheckIntervalMs - 1)); + expectAction(I2cFaultAction::Check, tracker.due(100 + kI2cCheckIntervalMs)); +} + +void test_one_failed_check_is_not_a_fault() { + I2cFaultTracker tracker; + TEST_ASSERT_FALSE(tracker.noteCheck(false, 1000)); + TEST_ASSERT_FALSE(tracker.down()); + TEST_ASSERT_EQUAL_UINT32(0, tracker.faults()); + + // A good check in between resets the count. + tracker.noteCheck(true, 1500); + TEST_ASSERT_FALSE(tracker.noteCheck(false, 2000)); + TEST_ASSERT_FALSE(tracker.down()); +} + +void test_consecutive_failures_take_the_bus_down() { + I2cFaultTracker tracker; + TEST_ASSERT_FALSE(tracker.noteCheck(false, 1000)); + TEST_ASSERT_TRUE(tracker.noteCheck(false, 1500)); + TEST_ASSERT_TRUE(tracker.down()); + TEST_ASSERT_EQUAL_UINT32(1, tracker.faults()); + TEST_ASSERT_EQUAL_UINT32(1500, tracker.lastFaultMs()); +} + +void test_first_recovery_attempt_is_immediate() { + I2cFaultTracker tracker; + const uint32_t at = takeDown(tracker, 1000); + expectAction(I2cFaultAction::Recover, tracker.due(at)); +} + +void test_failed_recovery_backs_off_to_the_cap() { + I2cFaultTracker tracker; + uint32_t now = takeDown(tracker, 1000); + + tracker.noteRecovery(false, now); + TEST_ASSERT_EQUAL_UINT32(kI2cFirstRetryMs, tracker.retryDelayMs()); + expectAction(I2cFaultAction::None, tracker.due(now + kI2cFirstRetryMs - 1)); + expectAction(I2cFaultAction::Recover, tracker.due(now + kI2cFirstRetryMs)); + + uint32_t previous = tracker.retryDelayMs(); + for (int i = 0; i < 12; i++) { + now += tracker.retryDelayMs(); + tracker.noteRecovery(false, now); + TEST_ASSERT_TRUE(tracker.retryDelayMs() >= previous); + TEST_ASSERT_TRUE(tracker.retryDelayMs() <= kI2cMaxRetryMs); + previous = tracker.retryDelayMs(); + } + + TEST_ASSERT_EQUAL_UINT32(kI2cMaxRetryMs, tracker.retryDelayMs()); + TEST_ASSERT_TRUE(tracker.down()); + TEST_ASSERT_EQUAL_UINT32(13, tracker.attempts()); + TEST_ASSERT_EQUAL_UINT32(0, tracker.recoveries()); +} + +void test_recovery_brings_the_bus_up_and_resumes_checks() { + I2cFaultTracker tracker; + const uint32_t at = takeDown(tracker, 1000); + + tracker.noteRecovery(false, at); + tracker.noteRecovery(true, at + kI2cFirstRetryMs); + + TEST_ASSERT_FALSE(tracker.down()); + TEST_ASSERT_EQUAL_UINT32(1, tracker.recoveries()); + TEST_ASSERT_EQUAL_UINT32(2, tracker.attempts()); + expectAction(I2cFaultAction::None, tracker.due(at + kI2cFirstRetryMs)); + expectAction( + I2cFaultAction::Check, + tracker.due(at + kI2cFirstRetryMs + kI2cCheckIntervalMs) + ); +} + +void test_second_fault_starts_over() { + I2cFaultTracker tracker; + uint32_t now = takeDown(tracker, 1000); + tracker.noteRecovery(false, now); + tracker.noteRecovery(false, now + kI2cFirstRetryMs); + tracker.noteRecovery(true, now + 3 * kI2cFirstRetryMs); + + now = takeDown(tracker, 60000); + TEST_ASSERT_TRUE(tracker.down()); + TEST_ASSERT_EQUAL_UINT32(2, tracker.faults()); + TEST_ASSERT_EQUAL_UINT32(0, tracker.attempts()); + // Back to an immediate first attempt, not the old backoff. + expectAction(I2cFaultAction::Recover, tracker.due(now)); +} + +void test_a_fault_takes_the_bus_down_at_once() { + I2cFaultTracker tracker; + tracker.noteCheck(true, 1000); + + TEST_ASSERT_TRUE(tracker.noteFault(1200)); + TEST_ASSERT_TRUE(tracker.down()); + TEST_ASSERT_EQUAL_UINT32(1, tracker.faults()); + TEST_ASSERT_EQUAL_UINT32(1200, tracker.lastFaultMs()); + expectAction(I2cFaultAction::Recover, tracker.due(1200)); +} + +void test_a_fault_while_down_is_not_counted_again() { + I2cFaultTracker tracker; + const uint32_t at = takeDown(tracker, 1000); + tracker.noteRecovery(false, at); + + TEST_ASSERT_FALSE(tracker.noteFault(at + 10)); + TEST_ASSERT_EQUAL_UINT32(1, tracker.faults()); + TEST_ASSERT_EQUAL_UINT32(at, tracker.lastFaultMs()); + // The backoff the failed attempt started still holds. + expectAction(I2cFaultAction::None, tracker.due(at + kI2cFirstRetryMs - 1)); +} + +void test_a_fault_before_the_first_check_still_recovers() { + I2cFaultTracker tracker; + TEST_ASSERT_TRUE(tracker.noteFault(50)); + expectAction(I2cFaultAction::Recover, tracker.due(50)); + + tracker.noteRecovery(true, 60); + TEST_ASSERT_FALSE(tracker.down()); + TEST_ASSERT_EQUAL_UINT32(1, tracker.recoveries()); +} + +void test_intervals_survive_millis_wrap() { + I2cFaultTracker tracker; + const uint32_t beforeWrap = UINT32_MAX - 100; + tracker.noteCheck(true, beforeWrap); + expectAction(I2cFaultAction::None, tracker.due(beforeWrap + 200)); + expectAction(I2cFaultAction::Check, tracker.due(beforeWrap + kI2cCheckIntervalMs)); + + takeDown(tracker, beforeWrap + kI2cCheckIntervalMs); + const uint32_t failedAt = beforeWrap + 2 * kI2cCheckIntervalMs; + tracker.noteRecovery(false, failedAt); + expectAction(I2cFaultAction::None, tracker.due(failedAt + kI2cFirstRetryMs - 1)); + expectAction(I2cFaultAction::Recover, tracker.due(failedAt + kI2cFirstRetryMs)); +} + +int main(int, char**) { + UNITY_BEGIN(); + RUN_TEST(test_checks_on_first_call_then_on_interval); + RUN_TEST(test_one_failed_check_is_not_a_fault); + RUN_TEST(test_consecutive_failures_take_the_bus_down); + RUN_TEST(test_first_recovery_attempt_is_immediate); + RUN_TEST(test_failed_recovery_backs_off_to_the_cap); + RUN_TEST(test_recovery_brings_the_bus_up_and_resumes_checks); + RUN_TEST(test_second_fault_starts_over); + RUN_TEST(test_a_fault_takes_the_bus_down_at_once); + RUN_TEST(test_a_fault_while_down_is_not_counted_again); + RUN_TEST(test_a_fault_before_the_first_check_still_recovers); + RUN_TEST(test_intervals_survive_millis_wrap); + return UNITY_END(); +}