diff --git a/.gitattributes b/.gitattributes index dfe077042..72dad2a0c 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,2 +1,3 @@ # Auto detect text files and perform LF normalization * text=auto +*.bat text eol=crlf diff --git a/.github/workflows/SITL.yml b/.github/workflows/SITL.yml new file mode 100644 index 000000000..1d3ef866b --- /dev/null +++ b/.github/workflows/SITL.yml @@ -0,0 +1,123 @@ +name: SITL Tests + +on: + push: + branches: ["main", "ark-release"] + pull_request: + branches: ["main", "ark-release"] + workflow_dispatch: + +jobs: + sitl-tests: + runs-on: ubuntu-latest + timeout-minutes: 30 + steps: + - uses: actions/checkout@v4 + + - name: Build SITL + run: make AM32_SITL_CAN + + - name: Install CI Python deps + run: | + python3 -m venv sitl-venv + sitl-venv/bin/pip install -U pip + sitl-venv/bin/pip install -r Mcu/SITL/requirements-ci.txt + + - name: Run SITL pytest suite + run: | + mkdir sitl_test_run && cd sitl_test_run + ../sitl-venv/bin/python ../Mcu/SITL/run_ci_tests.py \ + --sitl ../obj/AM32_AM32_SITL_CAN_*.elf \ + --junitxml=sitl-results.xml + + - name: Upload SITL log on failure + if: failure() + uses: actions/upload-artifact@v4 + with: + name: sitl-test-log + path: | + sitl_test_run/sitl_ci.log + sitl_test_run/**/sitl_ci.log + sitl_test_run/sitl-results.xml + if-no-files-found: ignore + + - name: Upload junit results + if: always() + uses: actions/upload-artifact@v4 + with: + name: sitl-junit + path: sitl_test_run/sitl-results.xml + if-no-files-found: ignore + + gui-test: + runs-on: ubuntu-latest + timeout-minutes: 20 + steps: + - uses: actions/checkout@v4 + + - name: Install Qt system libraries + run: sudo apt-get update && sudo apt-get install -y libgl1 libegl1 libfontconfig1 libxkbcommon0 + + - name: Build SITL + run: make AM32_SITL_CAN + + - name: Create GUI environment + run: python3 Mcu/SITL/make_gui_env.py + + - name: Run GUI test (offscreen) + run: | + mkdir gui_test_run && cd gui_test_run + python3 ../Mcu/SITL/gui_ci_test.py \ + --gui-python ../Mcu/SITL/venv/bin/python3 \ + --sitl ../obj/AM32_AM32_SITL_CAN_*.elf + + - name: Upload SITL log on failure + if: failure() + uses: actions/upload-artifact@v4 + with: + name: gui-test-log + path: gui_test_run/sitl_ci.log + if-no-files-found: ignore + + windows-build: + runs-on: windows-latest + timeout-minutes: 20 + steps: + - uses: actions/checkout@v4 + + - name: Install Cygwin + uses: cygwin/cygwin-install-action@v4 + with: + packages: gcc-core make + + - name: Build SITL + shell: C:\cygwin\bin\bash.exe --noprofile --norc -o igncr -eo pipefail '{0}' + run: | + cd $(cygpath -u "$GITHUB_WORKSPACE") + make AM32_SITL_CAN + ls -la obj/ + + - name: Boot smoke test + shell: C:\cygwin\bin\bash.exe --noprofile --norc -o igncr -eo pipefail '{0}' + run: | + cd $(cygpath -u "$GITHUB_WORKSPACE") + ./obj/AM32_AM32_SITL_CAN_*.elf --can-uri none --input-type 1 >boot.log 2>&1 & + sleep 3 + kill %1 || true + cat boot.log + grep -q "PWM/DShot input on udp port" boot.log + grep -q "state/model port udp" boot.log + + - name: Package binary with cygwin runtime + shell: C:\cygwin\bin\bash.exe --noprofile --norc -o igncr -eo pipefail '{0}' + run: | + cd $(cygpath -u "$GITHUB_WORKSPACE") + mkdir -p sitl-windows + cp obj/AM32_AM32_SITL_CAN_*.elf sitl-windows/AM32_SITL_CAN.exe + cp /bin/cygwin1.dll sitl-windows/ + + - name: Upload Windows SITL binary + uses: actions/upload-artifact@v4 + with: + name: sitl-windows + path: sitl-windows/ diff --git a/.github/workflows/hwci.yml b/.github/workflows/hwci.yml new file mode 100644 index 000000000..4a5336c38 --- /dev/null +++ b/.github/workflows/hwci.yml @@ -0,0 +1,131 @@ +name: Hardware-in-the-loop CI + +# Runs the AM32 firmware on a real ARK 4IN1 ESC mounted on a Tyto Robotics +# Flight Stand, capturing CPU load / loop times (over SWD) and efficiency / +# demag (thrust stand), then gates the result against a committed baseline. +# +# MANUAL TRIGGER ONLY (workflow_dispatch). The rig needs hands-on preparation +# before every run - battery connected and charged, prop/torque arm checked, +# Flight Stand Software up - so a human always starts the run; nothing fires +# automatically from pushes, PRs, or labels. This also means fork code never +# reaches the self-hosted bench machine unless a maintainer explicitly enters +# a PR number in the dispatch form. +# +# Requires a self-hosted runner labelled [self-hosted, hwci] that is +# physically wired to the rig and provisioned per hwci/README.md +# (ARM toolchain, OpenOCD, ST-Link udev rules, Flight Stand Software + gRPC, +# a rig config at $HWCI_RIG_CONFIG). + +on: + workflow_dispatch: + inputs: + profile: + description: Test profile to run + type: choice + default: ci_smoke + options: [ci_smoke, efficiency_sweep, demag_step_stress] + pr: + description: "PR number to test (optional: checks out that PR's head instead of the selected branch)" + type: string + default: "" + battery_cells: + description: "Battery cell count on the bench right now (e.g. 6 for 6S); the run refuses to start if pack voltage is too low for this. Leave blank to skip the check." + type: string + default: "6" + save_baseline: + description: Save this run's metrics as the new baseline (artifact) + type: boolean + default: false + +concurrency: + group: hwci-hardware # one job at a time owns the physical rig + cancel-in-progress: false + +jobs: + hil: + runs-on: [self-hosted, hwci] + timeout-minutes: 30 + env: + TARGET: ARK_4IN1_F051 + PROFILE: ${{ inputs.profile }} + BATTERY_CELLS: ${{ inputs.battery_cells }} + # Bench-specific config lives on the runner, not in the repo. + HWCI_RIG_CONFIG: ${{ vars.HWCI_RIG_CONFIG }} + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + ref: ${{ inputs.pr != '' && format('refs/pull/{0}/head', inputs.pr) || github.ref }} + + - name: Ensure ARM toolchain + shell: bash + run: | + # make/tools.mk pins ARM_SDK_PREFIX to tools/linux//bin, so a + # system arm-none-eabi-gcc on PATH does NOT satisfy the build; install + # the pinned SDK whenever the pinned path is missing. + if ! ls tools/linux/*/bin/arm-none-eabi-gcc >/dev/null 2>&1; then + make arm_sdk_install + fi + + - name: Install harness + shell: bash + working-directory: hwci + run: | + python3 -m venv .venv + . .venv/bin/activate + pip install -e '.[plot,flightstand]' + + - name: Run hardware CI + id: hil + shell: bash + working-directory: hwci + run: | + . .venv/bin/activate + RUN_DIR="runs/${GITHUB_SHA::8}-${PROFILE}" + echo "run_dir=hwci/$RUN_DIR" >> "$GITHUB_OUTPUT" + # Blank input -> no flag at all (hwci ci makes --battery-cells opt-in). + BATTERY_ARGS=() + if [ -n "$BATTERY_CELLS" ]; then + BATTERY_ARGS=(--battery-cells "$BATTERY_CELLS") + fi + # A missing baseline (first run on a fresh repo) skips the gate with + # a warning instead of failing, so save_baseline can bootstrap it. + set +e + hwci ci \ + --profile "$PROFILE" \ + --config "${HWCI_RIG_CONFIG:-$HOME/hwci-rig.yaml}" \ + --baseline "baselines/${TARGET}.json" \ + "${BATTERY_ARGS[@]}" \ + --out "$RUN_DIR" + rc=$? + set -e + echo "rc=$rc" >> "$GITHUB_OUTPUT" + exit $rc + + - name: Save baseline (optional) + # Runs even when the gate FAILed (rc=1: intentional re-baseline after an + # accepted change), but never after an aborted/crashed run (rc >= 2). + if: ${{ always() && inputs.save_baseline && contains(fromJSON('["0", "1"]'), steps.hil.outputs.rc) }} + shell: bash + working-directory: hwci + run: | + . .venv/bin/activate + hwci baseline-save "runs/${GITHUB_SHA::8}-${PROFILE}" \ + --out "baselines/${TARGET}.json" + + - name: Report to job summary + if: always() + shell: bash + run: | + RD="${{ steps.hil.outputs.run_dir }}" + if [ -f "$RD/report.md" ]; then cat "$RD/report.md" >> "$GITHUB_STEP_SUMMARY"; fi + + - name: Upload run data + if: always() + uses: actions/upload-artifact@v4 + with: + name: hwci-${{ env.PROFILE }}-${{ github.sha }} + path: | + ${{ steps.hil.outputs.run_dir }}/ + hwci/baselines/${{ env.TARGET }}.json + retention-days: 90 diff --git a/.gitignore b/.gitignore index 2d349113b..0659a0f4b 100644 --- a/.gitignore +++ b/.gitignore @@ -21,3 +21,10 @@ Keil_Projects/Listings /Debug/ windows-tools.zip .settings/ +Mcu/SITL/venv/ +.pytest_cache/ +**/__pycache__/ +**/sitl_ci.log +am32_eeprom.bin +am32_eeprom.bin.lock +am32_eeprom.bin.rtc diff --git a/Inc/functions.h b/Inc/functions.h index 6334015a5..bd4211b4f 100644 --- a/Inc/functions.h +++ b/Inc/functions.h @@ -11,6 +11,27 @@ #include "main.h" #include "targets.h" +/* + get current value of UTILITY_TIMER timer as 16bit microseconds + */ +static inline uint16_t get_timer_us16(void) +{ +#if defined(STMICRO) + return UTILITY_TIMER->CNT; +#elif defined(GIGADEVICES) + return TIMER_CNT(UTILITY_TIMER); +#elif defined(ARTERY) + return UTILITY_TIMER->cval; +#elif defined(NXP) + // Return nothing since NXP micro-tick works differently + return 0; +#elif defined(WCH) + return UTILITY_TIMER->CNT >> 1; +#else +#error unsupported MCU +#endif +} + uint32_t getAbsDif(int number1, int number2); void delayMicros(uint32_t micros); void delayMillis(uint32_t millis); diff --git a/Inc/hwci_perf.h b/Inc/hwci_perf.h new file mode 100644 index 000000000..8650ba50e --- /dev/null +++ b/Inc/hwci_perf.h @@ -0,0 +1,278 @@ +/* + * hwci_perf.h - Hardware-CI performance instrumentation for AM32 + * + * Optional, compile-gated (HWCI_PERF) instrumentation that maintains a small + * struct in RAM with live timing / CPU-load / health counters. A debugger + * (ST-Link via OpenOCD, or J-Link) reads this struct non-intrusively over SWD + * background memory access while the motor runs. + * + * WHY a RAM struct instead of SWO/ITM trace: + * The ARK 4IN1 ESC uses the STM32F051 (Cortex-M0 / ARMv6-M). The M0 has no + * DWT cycle counter, no ITM, and no SWO output, so trace-based profiling is + * impossible on ANY probe. Background memory reads of an instrumented struct + * are the portable way to get CPU load and loop times off this core. + * + * When HWCI_PERF is NOT defined, every macro below expands to a no-op and no + * struct or code is emitted - production builds are byte-for-byte unaffected. + * + * Timestamp source: the free-running 1 MHz utility timer, read through + * get_timer_us16() (Inc/functions.h), which handles each vendor family's + * register layout. 16-bit (wraps every 65.536 ms); all loop/ISR durations + * measured here are far below the wrap period, so 16-bit unsigned subtraction + * is exact. Values recorded across a >65 ms blocking event (startup/arming + * tunes play with IRQs off inside the control loop) alias - the armed-edge + * reset below discards those before a run's measurements start. + */ +#ifndef HWCI_PERF_H_ +#define HWCI_PERF_H_ + +#ifdef HWCI_PERF + +#if defined(NXP) || defined(WCH) +/* NXP has no free-running 1 MHz utility timer (get_timer_us16() returns 0). + * On WCH the utility timer doubles as INTERVAL_TIMER, which main.c zeroes at + * every zero-cross, so mid-measurement resets would corrupt every reading. */ +#error "HWCI_PERF is not supported on this MCU family (needs a free-running 1 MHz utility timer)" +#endif + +#include + +/* ASCII "HWC1" in little-endian memory order - lets the host locate/validate + * the struct either by ELF symbol or by scanning RAM for the magic. */ +#define HWCI_PERF_MAGIC 0x31435748u +/* v2: appended the zero-cross jitter block (zc_*) after host_cmd. + * v3: appended bidirectional DShot (BDShot) RX/TX health counters. */ +#define HWCI_PERF_VERSION 3u + +/* Commands the host may write to hwci_perf.host_cmd (cleared by firmware). */ +#define HWCI_CMD_NONE 0u +#define HWCI_CMD_RESET_STATS 0xA5u /* clear the min/max accumulators */ + +/* + * Naturally-aligned layout (NOT packed): Cortex-M0 faults on unaligned word + * access, so all 32-bit fields sit on 4-byte boundaries and 16-bit fields on + * 2-byte boundaries. The explicit offsets below are the contract the host + * decoder (hwci/hwci/perf.py) mirrors. Keep them in sync and bump + * HWCI_PERF_VERSION on any layout change. + */ +typedef struct hwci_perf_s { + uint32_t magic; /* off 0 : HWCI_PERF_MAGIC */ + uint16_t version; /* off 4 : HWCI_PERF_VERSION */ + uint16_t size; /* off 6 : sizeof(hwci_perf_t) */ + + /* --- loop timing, microseconds (UTILITY_TIMER 1us tick) --- */ + uint16_t ctrl_exec_us_last; /* off 8 : last tenKhzRoutine run */ + uint16_t ctrl_exec_us_max; /* off 10 : worst-case run */ + uint16_t ctrl_period_us_last; /* off 12 : last entry-to-entry gap */ + uint16_t ctrl_period_us_max; /* off 14 : worst-case gap (jitter) */ + uint16_t ctrl_period_us_min; /* off 16 : best-case gap */ + uint16_t main_loop_us_last; /* off 18 : last while(1) iteration */ + uint16_t main_loop_us_max; /* off 20 : worst-case iteration */ + + /* --- live state snapshot (mirrors telemetry for self-contained logs) --- */ + uint16_t input; /* off 22 : throttle input 0..2047 */ + uint16_t duty_cycle; /* off 24 : applied duty 0..2000 */ + uint16_t e_rpm; /* off 26 : electrical rpm / 100 */ + uint16_t voltage_cv; /* off 28 : battery, centivolts */ + int16_t current_ca; /* off 30 : current, centiamps */ + int16_t temperature_c; /* off 32 : MCU/FET temp, Celsius */ + uint8_t bemf_timeout_state; /* off 34 : bemf_timeout_happened */ + uint8_t armed; /* off 35 : armed flag */ + uint8_t running; /* off 36 : running flag */ + uint8_t _pad0; /* off 37 : alignment */ + uint16_t _pad1; /* off 38 : align next u32 to off 40 */ + + /* --- counters --- + * loop_iters is monotonic (the host differences it vs wall-clock for the + * idle-residual CPU-load estimate). zero_cross_count mirrors main.c's + * zero_crosses, which SATURATES at 10000 and resets on desync/stop - it + * is a diagnostic value, NOT a monotonic counter. update_count increments + * once per snapshot (every HWCI_PERF_SNAPSHOT_DIV main-loop iterations). + */ + uint32_t loop_iters; /* off 40 : while(1) iterations */ + uint32_t zero_cross_count; /* off 44 : zero_crosses mirror (sat)*/ + uint32_t commutation_interval; /* off 48 : raw, 0.5us units */ + uint32_t commutation_interval_max; /* off 52 : worst-case (slowest) */ + uint32_t update_count; /* off 56 : ++ each snapshot */ + volatile uint32_t host_cmd; /* off 60 : host writes, fw clears */ + + /* --- v2: zero-cross timing jitter (fed by HWCI_PERF_ZC) --- + * Appended AFTER host_cmd so its offset (60) is identical in v1 and v2 + * firmware: during an A/B session the host must be able to issue + * RESET_STATS to whichever vintage is flashed without a layout lookup. */ + uint32_t zc_count; /* off 64 : commutations accumulated */ + uint32_t zc_jitter_sum; /* off 68 : sum |deviation|, ticks */ + uint32_t zc_interval_sum; /* off 72 : sum raw interval, ticks */ + uint16_t zc_jitter_max; /* off 76 : worst single deviation */ + uint16_t _pad2; /* off 78 : keep sizeof 4-aligned */ + + /* --- v3: bidirectional DShot (BDShot) health --- + * Separates "FC never enabled BDShot" from "ESC RX CRC death" from + * "ESC TX'd eRPM but host decode failed". Monotonic u32 counters are + * host-diffed like loop_iters (wrap-safe). telem_mode is set when the + * idle-high auto-detect latches dshot_telemetry; edt_mode tracks the + * EDT enable command path. last_com_us is the period packed into the + * most recent reply (0.5 us units of e_com_time, or 65535 when stopped). + * Updated from dshot.c under HWCI_PERF only (zero cost when off). */ + uint32_t dshot_rx_good; /* off 80 : good-CRC frames */ + uint32_t dshot_rx_bad; /* off 84 : bad-CRC frames */ + uint32_t dshot_tx_frames; /* off 88 : BDShot reply packages */ + uint16_t dshot_last_com_us; /* off 92 : last packed com period */ + uint8_t dshot_telem_mode; /* off 94 : 0=uni DShot, 1=BDShot */ + uint8_t dshot_edt_mode; /* off 95 : EDT enabled */ +} hwci_perf_t; /* total size: 96 bytes */ + +extern volatile hwci_perf_t hwci_perf; + +/* Apply a pending host_cmd (e.g. reset accumulators). Defined in hwci_perf.c. */ +void hwci_perf_apply_cmd(void); + +/* Clear the sticky min/max accumulators (host command, and automatically on + * the armed 0->1 edge to discard aliased values recorded while the arming + * tune blocked the control loop). Defined in hwci_perf.c. */ +void hwci_perf_reset_stats(void); + +/* 16-bit microsecond timestamp from the free-running utility timer. Macro (not + * inline) so it is only evaluated where Inc/functions.h is already included + * (the struct definition above stays compilable on a host compiler). */ +#define HWCI_NOW_US() get_timer_us16() + +/* + * Control-loop (tenKhzRoutine, 20 kHz) instrumentation. ENTER at the very top, + * EXIT at the very bottom. ENTER records the period (entry-to-entry); EXIT + * records execution time (entry-to-exit). Runs in ISR context - keep it tiny. + */ +#define HWCI_PERF_CTRL_ENTER() \ + uint16_t _hwci_ctrl_t0 = HWCI_NOW_US(); \ + do { \ + static uint16_t _hwci_last_entry; \ + static uint8_t _hwci_ctrl_init; \ + if (_hwci_ctrl_init) { \ + uint16_t _p = (uint16_t)(_hwci_ctrl_t0 - _hwci_last_entry); \ + hwci_perf.ctrl_period_us_last = _p; \ + if (_p > hwci_perf.ctrl_period_us_max) hwci_perf.ctrl_period_us_max = _p; \ + if (_p < hwci_perf.ctrl_period_us_min) hwci_perf.ctrl_period_us_min = _p; \ + } \ + _hwci_last_entry = _hwci_ctrl_t0; \ + _hwci_ctrl_init = 1; \ + } while (0) + +#define HWCI_PERF_CTRL_EXIT() \ + do { \ + uint16_t _e = (uint16_t)(HWCI_NOW_US() - _hwci_ctrl_t0); \ + hwci_perf.ctrl_exec_us_last = _e; \ + if (_e > hwci_perf.ctrl_exec_us_max) hwci_perf.ctrl_exec_us_max = _e; \ + } while (0) + +/* + * Zero-cross jitter instrumentation. Call once per commutation, at the END of + * PeriodElapsedCallback() - never from interruptRoutine(), so the comparator + * ISR whose detection timing this metric characterizes is not perturbed by + * the act of measuring it. When PeriodElapsedCallback runs, thiszctime holds + * the newest raw zero-cross interval (INTERVAL_TIMER ticks; interruptRoutine + * zeroes the timer at every accepted crossing). + * + * The deviation is taken against the interval SIX commutations earlier - the + * same motor phase and comparator edge one electrical revolution back. + * Adjacent steps differ systematically (phase/comparator-edge asymmetry), so + * a tick-to-tick delta would bury detection noise under that fixed + * alternation; the 6-back reference cancels it. + * + * Accumulation is gated on zero_crosses >= 100, the same "stable running" + * threshold the zero-cross filter tiers use: startup seeds (interval forced + * to 10000) and post-desync recovery (firmware zeroes zero_crosses on + * desync) stay out of the sums, and the history ring refills during the + * gated-out span. zc_count/zc_jitter_sum/zc_interval_sum are monotonic - the + * host differences consecutive SWD snapshots (wrap-safe u32, like + * loop_iters), so delta(jitter_sum)/delta(interval_sum) over a window is the + * mean fractional jitter with every commutation counted, immune to the 200 Hz + * host sampling rate. zc_jitter_max is sticky and cleared alongside the other + * maxima by hwci_perf_reset_stats(). + */ +#define HWCI_PERF_ZC() \ + do { \ + static uint16_t _zc_hist[6]; \ + static uint8_t _zc_idx; \ + uint16_t _t = thiszctime; \ + uint16_t _ref = _zc_hist[_zc_idx]; \ + _zc_hist[_zc_idx] = _t; \ + if (++_zc_idx == 6u) _zc_idx = 0u; \ + if (zero_crosses >= 100) { \ + uint16_t _d = (_t >= _ref) ? (uint16_t)(_t - _ref) \ + : (uint16_t)(_ref - _t); \ + hwci_perf.zc_count++; \ + hwci_perf.zc_jitter_sum += _d; \ + hwci_perf.zc_interval_sum += _t; \ + if (_d > hwci_perf.zc_jitter_max) hwci_perf.zc_jitter_max = _d; \ + } \ + } while (0) + +/* + * Background-loop instrumentation. Call once at the top of the main while(1). + * + * Every iteration: measures iteration time and counts iterations (the host + * derives CPU load from the iteration rate vs an idle baseline), so the two + * hot signals stay exact. + * + * Every HWCI_PERF_SNAPSHOT_DIV-th iteration only: snapshots live state and + * services host commands. The host samples the struct at <= 200 Hz while the + * main loop runs at ~100 kHz, so refreshing the snapshot every iteration would + * burn ~10-15% of the core's spare cycles on stores that are overwritten + * unread - and, worse, depress the idle loop_iters rate that IS the CPU-load + * reference. At DIV=64 the snapshot is never staler than ~1 ms. + * + * The snapshot references main.c globals, so this macro must be expanded + * where those globals are in scope (i.e. in main()). ISR-written globals are + * cached into a local before use so the compare and the store can't see two + * different values (a torn read would let the sticky max go backwards). + */ +#define HWCI_PERF_SNAPSHOT_DIV 64u + +#define HWCI_PERF_MAIN_LOOP() \ + do { \ + uint16_t _n = HWCI_NOW_US(); \ + static uint16_t _hwci_main_last; \ + static uint8_t _hwci_main_init; \ + static uint8_t _hwci_prev_armed; \ + if (_hwci_main_init) { \ + uint16_t _d = (uint16_t)(_n - _hwci_main_last); \ + hwci_perf.main_loop_us_last = _d; \ + if (_d > hwci_perf.main_loop_us_max) hwci_perf.main_loop_us_max = _d; \ + } \ + _hwci_main_last = _n; \ + _hwci_main_init = 1; \ + hwci_perf.loop_iters++; \ + if ((hwci_perf.loop_iters & (HWCI_PERF_SNAPSHOT_DIV - 1u)) == 0u) { \ + uint32_t _ci = commutation_interval; \ + uint8_t _armed = (uint8_t)armed; \ + hwci_perf.input = input; \ + hwci_perf.duty_cycle = duty_cycle; \ + hwci_perf.e_rpm = e_rpm; \ + hwci_perf.voltage_cv = battery_voltage; \ + hwci_perf.current_ca = actual_current; \ + hwci_perf.temperature_c = degrees_celsius; \ + hwci_perf.bemf_timeout_state = (uint8_t)bemf_timeout_happened; \ + hwci_perf.armed = _armed; \ + hwci_perf.running = (uint8_t)running; \ + hwci_perf.zero_cross_count = zero_crosses; \ + hwci_perf.commutation_interval = _ci; \ + if (_ci > hwci_perf.commutation_interval_max) \ + hwci_perf.commutation_interval_max = _ci; \ + if (_armed && !_hwci_prev_armed) \ + hwci_perf_reset_stats(); /* drop aliased arming-tune maxima */ \ + _hwci_prev_armed = _armed; \ + hwci_perf.update_count++; \ + if (hwci_perf.host_cmd != HWCI_CMD_NONE) hwci_perf_apply_cmd(); \ + } \ + } while (0) + +#else /* !HWCI_PERF - all hooks vanish, no struct, no code */ + +#define HWCI_PERF_CTRL_ENTER() do {} while (0) +#define HWCI_PERF_CTRL_EXIT() do {} while (0) +#define HWCI_PERF_ZC() do {} while (0) +#define HWCI_PERF_MAIN_LOOP() do {} while (0) + +#endif /* HWCI_PERF */ + +#endif /* HWCI_PERF_H_ */ diff --git a/Inc/targets.h b/Inc/targets.h index b1dcfc1d4..a85970bd8 100644 --- a/Inc/targets.h +++ b/Inc/targets.h @@ -1,5 +1,16 @@ +/* + ELF section placement for the flash layout tooling (app signature, + file name). Not meaningful for the SITL build and mach-o (macOS) has + a different section syntax, so it becomes a no-op there + */ +#ifdef __APPLE__ +#define AM32_FLASH_SECTION(name) +#else +#define AM32_FLASH_SECTION(name) __attribute__((section(name))) +#endif + #ifndef USE_MAKE // #define F031_DEV // #define FD6288_F051 @@ -353,6 +364,19 @@ #define MILLIVOLT_PER_AMP 9 #endif /// +#ifdef AM32_SITL_CAN +#define FIRMWARE_NAME "AM32 SITL" +#define FILE_NAME "AM32_SITL_CAN" +#define DRONECAN_SUPPORT 1 +#define DRONECAN_NODE_NAME "org.am32.sitl" +#define DEAD_TIME 80 +#define HARDWARE_GROUP_SITL_A +#define TARGET_STALL_PROTECTION_INTERVAL 20000 +#define TARGET_VOLTAGE_DIVIDER 110 +#define MILLIVOLT_PER_AMP 20 +#define CURRENT_OFFSET 0 +#endif + #ifdef REF_G431 #define FIRMWARE_NAME "Ref G431" #define FILE_NAME "REF_G431" @@ -4065,6 +4089,12 @@ #endif +#ifdef HARDWARE_GROUP_SITL_A + +#define MCU_SITL + +#endif + #ifdef HARDWARE_GROUP_G4_A #define MCU_G431 @@ -5563,6 +5593,30 @@ #endif +#ifdef MCU_SITL +// software in the loop simulation, emulating a G431 class MCU with the +// hardware replaced by a motor/battery simulation. See Mcu/SITL +#define STMICRO +#define CPU_FREQUENCY_MHZ 160 +#ifndef EEPROM_START_ADD +#define EEPROM_START_ADD (uint32_t)0x0800F800 +#endif +#define INTERVAL_TIMER TIM2 +#define TEN_KHZ_TIMER TIM6 +#define UTILITY_TIMER TIM17 +#define COM_TIMER TIM16 +#define APPLICATION_ADDRESS 0x08001000 +#define TARGET_MIN_BEMF_COUNTS 3 +#define COMPARATOR_IRQ SITL_IRQ_COMP +#define COM_TIMER_IRQ SITL_IRQ_COM +#define IC_DMA_IRQ_NAME SITL_IRQ_DMA +#define USE_ADC +#define DSHOT_PRIORITY_THRESHOLD 60 +// the SITL harness provides the real main(), the firmware main() is +// started by the harness under this name +#define main am32_main +#endif + #ifndef LOOP_FREQUENCY_HZ #define LOOP_FREQUENCY_HZ 20000 #endif diff --git a/Makefile b/Makefile index 95f8da7a8..3378630a5 100644 --- a/Makefile +++ b/Makefile @@ -24,7 +24,7 @@ include $(ROOT)/make/tools.mk # supported MCU types -MCU_TYPES := E230 F031 F051 F415 F421 G071 L431 G431 V203 G031 A153 +MCU_TYPES := E230 F031 F051 F415 F421 G071 L431 G431 V203 G031 A153 SITL MCU_TYPE := NONE @@ -51,6 +51,14 @@ CFLAGS_BASE += -Wall -Wundef -Wextra -Werror -Wno-unused-parameter -Wno-stringop CFLAGS_COMMON := $(CFLAGS_BASE) +# Hardware-CI performance instrumentation (opt-in, off by default). +# Build with `make HWCI_PERF=1` to emit the hwci_perf RAM struct that +# the hardware-CI harness (see hwci/) reads over SWD. Production/release builds +# leave this unset and are completely unaffected. +ifeq ($(HWCI_PERF),1) +CFLAGS_COMMON += -DHWCI_PERF +endif + # Linker options LDFLAGS_COMMON := -specs=nano.specs $(LIBS) -Wl,--gc-sections -Wl,--print-memory-usage @@ -68,7 +76,9 @@ has_can_suffix = $(findstring _CAN,$1) $(foreach MCU,$(MCU_TYPES),$(eval SVD_$(MCU) := $(wildcard $(HAL_FOLDER_$(MCU))/*.svd))) .PHONY : clean all binary $(foreach MCU,$(MCU_TYPES),$(call lc,$(MCU))) -ALL_TARGETS := $(foreach MCU,$(MCU_TYPES),$(TARGETS_$(MCU))) +# Host-native SITL is opt-in (`make AM32_SITL_CAN` / `make sitl`), not part of +# the cross-compiled `make all` matrix used by Linux firmware CI. +ALL_TARGETS := $(foreach MCU,$(filter-out SITL,$(MCU_TYPES)),$(TARGETS_$(MCU))) all : $(ALL_TARGETS) # create targets for compiling one mcu type, eg "make f421" @@ -86,7 +96,8 @@ clean : define CREATE_BUILD_TARGET $(2)_BASENAME = $(BIN_DIR)/$(IDENTIFIER)_$(2)_$(FIRMWARE_VERSION) -$(2) : $$($(2)_BASENAME).bin +# native (SITL) targets build to an executable elf, no bin/hex conversion +$(2) : $$($(2)_BASENAME).$(if $(NATIVE_$(1)),elf,bin) # get MCU specific compiler, objcopy and link script or use the ARM SDK one $(eval xCC := $(if $($(MCU)_CC), $($(MCU)_CC), $(CC))) @@ -106,21 +117,26 @@ $(eval xLDSCRIPT := $$(if $$(call has_can_suffix,$$(2)),$(LDSCRIPT_CAN_$(1)),$(L $(eval xCFLAGS := $$(if $$(call has_can_suffix,$$(2)),$(CFLAGS_CAN_$(1)))) $(eval xSRC := $$(if $$(call has_can_suffix,$$(2)),$(SRC_CAN_$(1)))) -CFLAGS_$(2) = -DAM32_MCU=\"$(MCU)\" $(MCU_$(1)) -D$(2) $(CFLAGS_$(1)) $(CFLAGS_COMMON) $(xCFLAGS) -LDFLAGS_$(2) = $(LDFLAGS_COMMON) $(LDFLAGS_$(1)) -T$(xLDSCRIPT) +# allow an MCU type to override the common compiler/linker flags (used by SITL +# for a native build) and to have no linker script +$(eval xCFLAGS_COMMON := $(if $(CFLAGS_COMMON_$(1)),$(CFLAGS_COMMON_$(1)),$(CFLAGS_COMMON))) +$(eval xLDFLAGS_COMMON := $(if $(LDFLAGS_COMMON_$(1)),$(LDFLAGS_COMMON_$(1)),$(LDFLAGS_COMMON))) + +CFLAGS_$(2) = -DAM32_MCU=\"$(MCU)\" $(MCU_$(1)) -D$(2) $(CFLAGS_$(1)) $(xCFLAGS_COMMON) $(xCFLAGS) +LDFLAGS_$(2) = $(xLDFLAGS_COMMON) $(LDFLAGS_$(1)) $(if $(xLDSCRIPT),-T$(xLDSCRIPT)) -include $$($(2)_BASENAME).d $$($(2)_BASENAME).elf: $(SRC_COMMON) $$(SRC_$(1)) $(xSRC) @$(ECHO) Compiling $$(notdir $$@) $(QUIET)$(MKDIR) -p $(OBJ) - $(QUIET)$(xCC) $$(CFLAGS_$(2)) $$(LDFLAGS_$(2)) -MMD -MP -MF $$(@:.elf=.d) -o $$(@) $(SRC_COMMON) $$(SRC_$(1)) $(xSRC) + $(QUIET)$(xCC) $$(CFLAGS_$(2)) $$(LDFLAGS_$(2)) -MMD -MP -MF $$(@:.elf=.d) -o $$(@) $(SRC_COMMON) $$(SRC_$(1)) $(xSRC) $(LDLIBS_$(1)) # we copy debug.elf to give us a constant debug target for vscode # this means the debug button will always debug the last target built - $(QUIET)$(CP) -f $$(SVD_$(1)) $(OBJ)/debug.svd + $(if $(SVD_$(1)),$(QUIET)$(CP) -f $$(SVD_$(1)) $(OBJ)/debug.svd) # also copy the openocd.cfg from the MCU directory to obj/openocd.cfg for auto config of Cortex-Debug # in vscode - $(QUIET)$(CP) -f Mcu$(DSEP)$(call lc,$(1))$(DSEP)openocd.cfg $(OBJ)$(DSEP)openocd.cfg > $(NUL) + $(if $(NATIVE_$(1)),,$(QUIET)$(CP) -f Mcu$(DSEP)$(call lc,$(1))$(DSEP)openocd.cfg $(OBJ)$(DSEP)openocd.cfg > $(NUL)) endef $(foreach MCU,$(MCU_TYPES),$(foreach TARGET,$(TARGETS_$(MCU)), $(eval $(call CREATE_BUILD_TARGET,$(MCU),$(TARGET))))) diff --git a/Mcu/SITL/Inc/ADC.h b/Mcu/SITL/Inc/ADC.h new file mode 100644 index 000000000..fee2cc051 --- /dev/null +++ b/Mcu/SITL/Inc/ADC.h @@ -0,0 +1,16 @@ +/* + * ADC.h - SITL + */ + +#include "main.h" +#include "targets.h" + +#ifndef ADC_H_ +#define ADC_H_ + +void ADC_DMA_Callback(void); +void enableADC_DMA(void); +void activateADC(void); +void ADC_Init(void); + +#endif /* ADC_H_ */ diff --git a/Mcu/SITL/Inc/IO.h b/Mcu/SITL/Inc/IO.h new file mode 100644 index 000000000..af70f338f --- /dev/null +++ b/Mcu/SITL/Inc/IO.h @@ -0,0 +1,27 @@ +/* + * IO.h - SITL (dshot/servo input is not supported, stubs only) + */ + +#ifndef IO_H_ +#define IO_H_ + +#include "main.h" + +void changeToOutput(void); +void changeToInput(void); +void receiveDshotDma(void); +void sendDshotDma(void); + +uint8_t getInputPinState(void); +void setInputPolarityRising(void); +void setInputPullDown(void); +void setInputPullUp(void); +void enableHalfTransferInt(void); +void setInputPullNone(void); + +extern volatile char inputSet; +extern char dshot; +extern volatile char servoPwm; +extern volatile char send_telemetry; + +#endif /* IO_H_ */ diff --git a/Mcu/SITL/Inc/comparator.h b/Mcu/SITL/Inc/comparator.h new file mode 100644 index 000000000..b52e265d7 --- /dev/null +++ b/Mcu/SITL/Inc/comparator.h @@ -0,0 +1,18 @@ +/* + * comparator.h - SITL + */ + +#ifndef COMPARATOR_H_ +#define COMPARATOR_H_ + +#include "main.h" + +uint8_t getCompOutputLevel(void); +void maskPhaseInterrupts(void); +void enableCompInterrupts(void); +void changeCompInput(void); + +extern volatile char rising; +extern char step; + +#endif /* COMPARATOR_H_ */ diff --git a/Mcu/SITL/Inc/main.h b/Mcu/SITL/Inc/main.h new file mode 100644 index 000000000..f9e0c02ca --- /dev/null +++ b/Mcu/SITL/Inc/main.h @@ -0,0 +1,60 @@ +/* + main.h for the SITL "MCU". Provides just enough of the STM32 LL/CMSIS + surface for the shared firmware sources to build natively, backed by the + emulation in Mcu/SITL/Src + */ + +#ifndef __MAIN_H +#define __MAIN_H + +#include +#include + +#include "sitl.h" +#include "sitl_nvic.h" + +// emulated timers. Dereferencing syncs CNT from simulated time +#define TIM1 sitl_tim_deref(SITL_TIM1_IDX) +#define TIM2 sitl_tim_deref(SITL_TIM2_IDX) +#define TIM6 sitl_tim_deref(SITL_TIM6_IDX) +#define TIM16 sitl_tim_deref(SITL_TIM16_IDX) +#define TIM17 sitl_tim_deref(SITL_TIM17_IDX) + +// minimal register structs so common.h externs compile +typedef struct { + volatile uint32_t IDR; + volatile uint32_t ODR; + volatile uint32_t BSRR; + volatile uint32_t BRR; +} GPIO_TypeDef; + +typedef struct { + volatile uint32_t CSR; +} COMP_TypeDef; + +extern GPIO_TypeDef sitl_gpio_dummy; +extern COMP_TypeDef sitl_comp_dummy[2]; +#define GPIOA (&sitl_gpio_dummy) +#define GPIOB (&sitl_gpio_dummy) +#define COMP1 (&sitl_comp_dummy[0]) +#define COMP2 (&sitl_comp_dummy[1]) + +// ADC: the simulation provides the raw values directly, conversion start +// is a no-op and the "temperature calculation" is a pass through (the +// simulation stores degrees C in ADC_raw_temp) +#define ADC1 0 +#define LL_ADC_REG_StartConversion(adc) do { (void)(adc); } while (0) +#define LL_ADC_RESOLUTION_12B 0 +#define __LL_ADC_CALC_TEMPERATURE(vref, raw, res) ((int32_t)(raw)) + +// watchdog +#define IWDG 0 +#define LL_IWDG_ReloadCounter(wdg) sitl_watchdog_reload() + +#ifndef RESET +#define RESET 0 +#endif + +void Error_Handler(void); + +#endif /* __MAIN_H */ diff --git a/Mcu/SITL/Inc/peripherals.h b/Mcu/SITL/Inc/peripherals.h new file mode 100644 index 000000000..7f3b9af8a --- /dev/null +++ b/Mcu/SITL/Inc/peripherals.h @@ -0,0 +1,46 @@ +/* + peripherals.h for the SITL MCU. Same macro API as the g431 version with + the register pokes routed into the emulation + */ + +#ifndef PERIPHERALS_H_ +#define PERIPHERALS_H_ +#endif /* PERIPHERALS_H_ */ + +#include "ADC.h" +#include "main.h" + +#define INTERVAL_TIMER_COUNT (sitl_interval_timer_count()) +#define RELOAD_WATCHDOG_COUNTER() sitl_watchdog_reload() +#define DISABLE_COM_TIMER_INT() sitl_com_int_disable() +#define ENABLE_COM_TIMER_INT() sitl_com_int_enable() +#define SET_AND_ENABLE_COM_INT(time) sitl_com_int_arm(time) +#define SET_INTERVAL_TIMER_COUNT(intertime) sitl_interval_timer_set(intertime) +#define SET_PRESCALER_PWM(presc) sitl_tim1_set_psc(presc) +#define SET_AUTO_RELOAD_PWM(relval) sitl_tim1_set_arr(relval) +#define SET_DUTY_CYCLE_ALL(newdc) sitl_tim1_set_duty_all(newdc) + +void initAfterJump(void); +void initCorePeripherals(void); +void SystemClock_Config(void); +void MX_GPIO_Init(void); +void MX_DMA_Init(void); +void MX_ADC1_Init(void); +void MX_COMP2_Init(void); +void MX_COMP1_Init(void); +void MX_TIM1_Init(void); +void MX_TIM2_Init(void); +void MX_TIM3_Init(void); +void MX_TIM14_Init(void); +void MX_TIM17_Init(void); +void MX_TIM16_Init(void); +void MX_IWDG_Init(void); +void MX_TIM6_Init(void); +void MX_TIM15_Init(void); +void resetInputCaptureTimer(void); +void setPWMCompare1(uint16_t compareone); +void setPWMCompare2(uint16_t comparetwo); +void setPWMCompare3(uint16_t comparethree); +void enableCorePeripherals(void); +void reloadWatchDogCounter(void); +void generatePwmTimerEvent(void); diff --git a/Mcu/SITL/Inc/phaseouts.h b/Mcu/SITL/Inc/phaseouts.h new file mode 100644 index 000000000..e816dfeab --- /dev/null +++ b/Mcu/SITL/Inc/phaseouts.h @@ -0,0 +1,18 @@ +/* + * phaseouts.h - SITL + */ + +#ifndef PHASEOUTS_H_ +#define PHASEOUTS_H_ + +#include "main.h" + +extern void allOff(void); +extern void comStep(int newStep); +extern void fullBrake(void); +extern void allpwm(void); +extern void proportionalBrake(void); +extern void twoChannelForward(void); +extern void twoChannelReverse(void); + +#endif /* PHASEOUTS_H_ */ diff --git a/Mcu/SITL/Inc/serial_telemetry.h b/Mcu/SITL/Inc/serial_telemetry.h new file mode 100644 index 000000000..2f226f590 --- /dev/null +++ b/Mcu/SITL/Inc/serial_telemetry.h @@ -0,0 +1,13 @@ +/* + * serial_telemetry.h - SITL stub + */ + +#include "main.h" + +#ifndef SERIAL_TELEMETRY_H_ +#define SERIAL_TELEMETRY_H_ + +void telem_UART_Init(void); +void send_telem_DMA(uint8_t bytes); + +#endif /* SERIAL_TELEMETRY_H_ */ diff --git a/Mcu/SITL/Inc/sitl.h b/Mcu/SITL/Inc/sitl.h new file mode 100644 index 000000000..486917d57 --- /dev/null +++ b/Mcu/SITL/Inc/sitl.h @@ -0,0 +1,174 @@ +/* + sitl.h - internal API of the AM32 SITL runtime + + The firmware runs unmodified in one thread ("firmware thread") while a + second thread ("sim thread") owns simulated time, the motor physics and + delivery of emulated interrupts. Interrupt handlers always execute in the + sim thread with the firmware thread suspended, giving the same + run-to-completion semantics as a real MCU. + + All state shared between the two threads is either written through the + helpers here or is a plain volatile word. x86 total-store-ordering is + assumed. + */ + +#pragma once + +#include +#include + +// emulated interrupt sources, in NVIC style. Default priorities are set to +// match the g431 target, main.c re-prioritises at runtime +enum sitl_irq { + SITL_IRQ_COMP = 0, // BEMF comparator EXTI + SITL_IRQ_COM, // commutation timer (TIM16) update + SITL_IRQ_TENKHZ, // 20kHz loop timer (TIM6) + SITL_IRQ_DMA, // input capture DMA (PWM/DShot over UDP) + SITL_IRQ_EXTI15, // software interrupt for dshot processing + SITL_IRQ_CAN, // CAN frame RX poll + SITL_IRQ_MAX +}; + +// PWM/DShot input over UDP (sitl_input.c) +void sitl_input_init(void); +void sitl_input_poll(void); // sim thread, every physics step +void sitl_input_arm(void); // receiveDshotDma() +void sitl_input_timer_reset(void); // resetInputCaptureTimer() +void sitl_input_send_reply(void); // sendDshotDma() +void sitl_input_dma_irq(void); // DMA transfer complete handler +uint8_t sitl_input_pin_state(void); // getInputPinState() +void sitl_input_stats(uint32_t out[4]); + +// simulation state streaming + runtime model control (sitl_state.c) +void sitl_state_init(void); +void sitl_state_poll(void); // sim thread, every 100us +void sitl_state_step(uint64_t now_ns); // sim thread, every physics step + +// simulated monotonic time since start +uint64_t sitl_time_ns(void); + +// a non blocking close-on-exec UDP socket (sitl_compat.c) +int sitl_udp_socket(void); + +// true when called from the sim thread (i.e. from interrupt context) +bool sitl_in_sim_thread(void); + +// advance the simulation by one physics step. Only legal from the sim +// thread; used so that busy loops inside interrupt handlers (delayMicros, +// comparator filter re-reads) still see time advance +void sitl_step_from_isr(void); + +// account for one register/comparator read from interrupt context. Each +// read costs sim.isr_read_ns (about what a peripheral read costs on the +// real MCU); whole physics steps run once enough time has accumulated +void sitl_isr_read_tick(void); + +// account for a register read from the firmware thread; grants simulated +// time while the firmware holds PRIMASK +void sitl_fw_read_tick(void); + + +// NVIC emulation +void sitl_nvic_set_priority(int irq, uint32_t prio); +void sitl_nvic_enable_irq(int irq); +void sitl_nvic_disable_irq(int irq); +void sitl_irq_pend(int irq); +void sitl_primask_set(void); // __disable_irq +void sitl_primask_clear(void); // __enable_irq +uint32_t sitl_primask_get(void); // __get_PRIMASK +void sitl_system_reset(void) __attribute__((noreturn)); + +// watchdog +void sitl_watchdog_reload(void); +void sitl_watchdog_enable(void); + +// timers (see sitl_timers.c) +enum sitl_tim_idx { + SITL_TIM1_IDX = 0, // PWM timer, 160MHz, ARR/CCR preloaded + SITL_TIM2_IDX, // INTERVAL_TIMER, 2MHz, 32 bit + SITL_TIM6_IDX, // TEN_KHZ_TIMER, 1MHz, periodic update + SITL_TIM16_IDX, // COM_TIMER, 2MHz, update interrupt + SITL_TIM17_IDX, // UTILITY_TIMER, 1MHz, 16 bit free running + SITL_NUM_TIMS +}; + +typedef struct { + volatile uint32_t CNT; + volatile uint32_t ARR; + volatile uint32_t PSC; + volatile uint32_t DIER; + volatile uint32_t SR; + volatile uint32_t BDTR; + volatile uint32_t CCR1; + volatile uint32_t CCR2; + volatile uint32_t CCR3; +} SITL_TIM_TypeDef; + +// dereference an emulated timer, syncing CNT from simulated time +SITL_TIM_TypeDef* sitl_tim_deref(int idx); + +uint32_t sitl_interval_timer_count(void); +void sitl_interval_timer_set(uint32_t cnt); +void sitl_com_int_arm(uint32_t time); +void sitl_com_int_disable(void); +void sitl_com_int_enable(void); +void sitl_tim1_set_duty_all(uint16_t duty); +void sitl_tim1_set_duty(int chan, uint16_t duty); +void sitl_tim1_set_psc(uint16_t psc); +void sitl_tim1_set_arr(uint16_t arr); +void sitl_tim1_force_update(void); +void sitl_tenkhz_enable(void); + +// called by the sim thread each physics step to check timer events +void sitl_timers_step(uint64_t now_ns); +void sitl_timers_init(void); + +// PWM output sampling for the physics (phase 0..2), true when the high +// side compare is active for the current TIM1 counter phase +bool sitl_tim1_pwm_out(int chan, uint64_t now_ns); + +// dead time in ns decoded from the emulated TIM1 BDTR DTG field +uint32_t sitl_tim1_dead_time_ns(void); + +// EXTI emulation for the comparator lines +#define SITL_EXTI_LINE_21 (1UL << 21) +#define SITL_EXTI_LINE_22 (1UL << 22) + +typedef struct { + volatile uint32_t IMR; + volatile uint32_t RTSR; + volatile uint32_t FTSR; + volatile uint32_t PR; +} sitl_exti_t; + +extern sitl_exti_t sitl_exti; + +// bridge output modes per phase, set by phaseouts.c, read by the physics +enum sitl_phase_mode { + SITL_PHASE_FLOAT = 0, // both fets off + SITL_PHASE_LOW, // low side on + SITL_PHASE_PWM, // high side pwm, low side complementary if comp_pwm + SITL_PHASE_PWM_NOCOMP, // high side pwm, low side off + SITL_PHASE_BRAKE_PWM, // low side driven by complementary pwm +}; + +extern volatile uint8_t sitl_phase_mode[3]; + +// comparator state: which phase is floating and the latched output +extern volatile uint8_t sitl_comp_phase; // 0=A 1=B 2=C +extern volatile uint8_t sitl_comp_out; + +// sensor snapshot from the physics, seqlock protected +typedef struct { + float bus_voltage; // V at the ESC input + float bus_current; // A into the bridge + float temperature_c; + float rpm; // mechanical, signed +} sitl_sensors_t; + +void sitl_sensors_read(sitl_sensors_t* out); +void sitl_sensors_write(const sitl_sensors_t* in); // sim thread only + +// lifecycle +void sitl_start_sim_thread(void); +extern char** sitl_saved_argv; diff --git a/Mcu/SITL/Inc/sitl_nvic.h b/Mcu/SITL/Inc/sitl_nvic.h new file mode 100644 index 000000000..ae9bc2090 --- /dev/null +++ b/Mcu/SITL/Inc/sitl_nvic.h @@ -0,0 +1,41 @@ +/* + sitl_nvic.h - CMSIS style interrupt controls mapped onto the SITL runtime + */ + +#pragma once + +#include "sitl.h" + +typedef int IRQn_Type; + +static inline void NVIC_SetPriority(IRQn_Type irq, uint32_t prio) +{ + sitl_nvic_set_priority(irq, prio); +} + +static inline void NVIC_EnableIRQ(IRQn_Type irq) +{ + sitl_nvic_enable_irq(irq); +} + +static inline void NVIC_DisableIRQ(IRQn_Type irq) +{ + sitl_nvic_disable_irq(irq); +} + +#define NVIC_SystemReset() sitl_system_reset() + +static inline void __disable_irq(void) +{ + sitl_primask_set(); +} + +static inline void __enable_irq(void) +{ + sitl_primask_clear(); +} + +static inline uint32_t __get_PRIMASK(void) +{ + return sitl_primask_get(); +} diff --git a/Mcu/SITL/README.md b/Mcu/SITL/README.md new file mode 100644 index 000000000..f420d1d8f --- /dev/null +++ b/Mcu/SITL/README.md @@ -0,0 +1,241 @@ +# AM32 SITL (software in the loop) + +Runs the AM32 firmware as a native Linux executable against a simulation +of the motor, bridge and battery, with DroneCAN input/output over +multicast UDP. This allows testing of the firmware logic (startup, +commutation, DroneCAN protocol, parameters) without ESC hardware. + +## Building + +``` +make AM32_SITL_CAN +``` + +produces `obj/AM32_AM32_SITL_CAN_.elf`, a normal Linux +executable. SITL is **not** part of `make all` (that target stays +cross-firmware only); use `make sitl` or the product name above. + +Also note: RTC backup registers used by DroneCAN boot/FW-update handoff +persist across SITL re-execs in a sibling file `.rtc` next to the +eeprom backing file. + +## Running + +``` +obj/AM32_AM32_SITL_CAN_*.elf --node-id 10 --verbose +``` + +Options: + +- `--config FILE` JSON file with motor/battery/esc/sim properties (see + `example.json`, all keys optional) +- `--eeprom FILE` eeprom backing file (default `am32_eeprom.bin`). A + missing file is seeded with the AM32 configurator default settings +- `--can-uri URI` CAN interface, default `mcast:0` (group 239.65.82.N + port 57732, wire compatible with libcanard and ArduPilot SITL). An + optional interface may be given (`mcast:0:lo`). `none` disables CAN, + for pure PWM/DShot testing +- `--node-id N` force the DroneCAN node ID, otherwise DNA is used +- `--input-port N` UDP port for PWM/DShot input (default 57733, 0 + disables) +- `--state-port N` UDP port for high rate simulation state streaming + and runtime motor model loading (default 57734, 0 disables) +- `--bind-any` bind the input and state ports on all interfaces instead + of loopback only, needed when the GUI runs on a different host. The + ports accept unauthenticated control, so only use on trusted networks +- `--input-type N` force the eeprom INPUT_SIGNAL_TYPE setting (0=auto + 1=dshot 2=servo 5=dronecan) +- `--speedup X` simulation speed relative to wall clock, 0 = free + running; clamped to `[0, 100]` (same as the GUI/state-port control) +- `--uid STR` string used to derive the 16 byte unique ID +- `--verbose` 1Hz state line on stderr +- `--nosleep` busy wait instead of sleeping. Uses two full CPU cores but + avoids OS sleep/wakeup latency for the most accurate wall clock pacing + +The virtual ESC can then be controlled with the DroneCAN GUI tool or +pydronecan on `mcast:0`. A test script is included: + +``` +python3 Mcu/SITL/sitl_can_test.py --throttle 0.5 --duration 20 +``` + +which arms, ramps the throttle via `esc.RawCommand` and reports the +`esc.Status` telemetry (RPM, voltage, current, temperature). + +Note that with no DroneCAN traffic directed at the node it will reboot +every 2 seconds from the firmware signal-timeout logic, exactly as real +hardware does. Reboots (including `RestartNode` and watchdog resets) +re-exec the process; the eeprom file persists. + +Each instance holds a lock on its eeprom file: two instances sharing an +eeprom (and therefore a node ID) would interleave their DroneCAN +transfers on the bus, which shows up as erratic telemetry. To run +multiple ESCs give each its own `--eeprom` and `--node-id`. + +## PWM/DShot input over UDP + +The SITL listens on a UDP port (default 57733) for PWM or DShot input +frames. Each packet is one frame on the virtual signal wire, synthesized +into input-capture edge timestamps and decoded by the firmware's +unmodified `Src/signal.c`/`Src/dshot.c` logic, including input type +auto-detection, CRC checking, zero-throttle arming, DShot commands and +bidirectional DShot auto-detect (idle high line). + +packet format (little endian): + +| field | size | meaning | +|-------|------|---------| +| magic | u16 | 0x4453 | +| type | u8 | 0=PWM 1=DSHOT150 2=DSHOT300 3=DSHOT600 | +| len | u8 | payload bytes after the header (4) | +| flags | u16 | bit0: line idle level (1 = idle high, bidir DShot) | +| data | u16 | PWM pulse width in us, or the full 16 bit DShot frame | + +Bidirectional DShot replies (eRPM plus extended telemetry frames) are +sent back to the most recent sender in the same format, with `data` +carrying the 16 bit GCR-decoded reply frame. + +Tools in `Mcu/SITL/`: + +- `sitl_gui.py` — Qt (PySide6) GUI driving both the PWM/DShot input and + DroneCAN input with per-input enable switches (for failover testing), + BDShot/EDT and esc.Status telemetry with rates, and an + `INPUT_SIGNAL_TYPE` parameter panel. The simulation panel selects the + motor model (the JSON files in `Mcu/SITL/models/`, applied to the + running simulation over the state port; switch at zero throttle for + clean results) and has optional high rate views, both default off: + pyqtgraph scopes of the phase currents and the phase terminal + voltages, each in its own window (sample period down to the 500ns + physics step and adjustable window; the sample rate is automatically + limited to about 200k samples/s of wall clock, so fine periods take + effect as the speedup is lowered — the PWM dead time diode conduction + is visible on the voltages at fine sample periods), and a motor/bridge + animation showing rotor angle, per phase bridge modes and the + comparator. A speedup slider (0.01x to 2x) + changes the simulation pace at runtime, for watching the animation in + slow motion; input frames arriving faster than the slowed simulation + consumes them are dropped, as on a real wire. `--control-port N` accepts UI + commands over a localhost TCP connection for scripted tests (default + off); `--log FILE` records every UI action with timestamps and + `--replay FILE` plays a recording back, so a failing interactive + session can be reproduced exactly. Install the + dependencies (PySide6, pyqtgraph, dronecan; Linux/Windows/macOS) into + a self-contained environment with + +``` +python3 Mcu/SITL/make_gui_env.py +``` + + which creates `Mcu/SITL/venv` and prints the interpreter to run the + GUI with. A system python with the packages from + `Mcu/SITL/requirements-gui.txt` installed works too. The UI backends + live in `sitl_gui_backend.py`, UI-independent for headless tests +- `dshot_test.py` — headless scripted test (arming, throttle, EDT, + bad-CRC injection), e.g.: + +``` +obj/AM32_AM32_SITL_CAN_*.elf --can-uri none --input-type 1 +python3 Mcu/SITL/dshot_test.py --type dshot600 --bidir --edt --throttle 800 +``` + +Note that the eeprom default `INPUT_SIGNAL_TYPE` is DRONECAN_IN, which +disables the PWM/DShot input interrupts at startup — set it to 0/1/2 +first (via `--input-type`, the GUI parameter panel, or +`dshot_test.py --input-type`). Also be aware of the current firmware +input arbitration: once any `esc.RawCommand` has been received, the 1kHz +DroneCAN input keep-alive overrides the `dshot`/`inputSet` flags and +PWM/DShot input is dead until a reboot (signal timeout after the CAN +stream stops) followed by zero-throttle re-arming. Running both inputs +at once exercises exactly this behaviour, which is what the input +priority/failover parameter work is developing against. + +## macOS + +Builds and runs natively (Apple Silicon or Intel) with the stock Xcode +command line tools: `make AM32_SITL_CAN`. The GUI bootstrap is the same +`python3 Mcu/SITL/make_gui_env.py`. Multicast CAN over loopback works +without configuration. + +## Windows + +The SITL builds under Cygwin (packages: gcc-core, make) with the same +`make AM32_SITL_CAN`, producing a native console executable, and the +POSIX signal based scheduler runs correctly under the Cygwin runtime. +The GUI uses a normal Windows python: `py Mcu/SITL/make_gui_env.py` +creates the environment and prints the interpreter to use; after that +`sitl_gui.bat` in the repository root launches the GUI (double click or +from cmd, extra arguments are passed through). Notes: + +- to run the binary from outside a Cygwin shell (cmd, double click), + copy `C:\cygwin64\bin\cygwin1.dll` next to it - its only Cygwin + dependency (the CI artifact ships it bundled). +- Windows Firewall must allow inbound UDP for the SITL binary (or ports + 57732-57734) for CAN and the input/state ports to receive. +- a socket never receives its own multicast on Windows, so the CAN TX + self test is skipped there; on a machine with several interfaces pass + an explicit one as `--can-uri mcast:0:`. + +## Headless / CI use + +Everything runs without a display: the SITL is a plain console binary +and the GUI works under Qt's offscreen platform +(`QT_QPA_PLATFORM=offscreen`) driven through `--control-port`, so full +interactive scenarios can run in CI. On a minimal Debian/Ubuntu the +requirements are: + +``` +apt install gcc make python3 python3-venv \ + libgl1 libegl1 libfontconfig1 libxkbcommon0 +python3 -m venv sitl-venv && sitl-venv/bin/pip install -r Mcu/SITL/requirements-ci.txt +python3 Mcu/SITL/make_gui_env.py # for GUI-driven tests +``` + +Build and run the pytest suite (boot, DShot/BDShot/EDT, PWM, DroneCAN +throttle + arming, parameter GetSet/save, motor model load): + +``` +make AM32_SITL_CAN +sitl-venv/bin/python Mcu/SITL/run_ci_tests.py +# or: sitl-venv/bin/pytest Mcu/SITL/tests -v --sitl obj/AM32_AM32_SITL_CAN_*.elf +``` + +`run_ci_tests.py` prefers pytest; pass `--legacy` for the smaller +stdlib-only smoke suite. The GitHub Actions workflow +`.github/workflows/SITL.yml` runs this on every push/PR to `main` and +`ark-release` (plus a GUI offscreen job and a Windows build/smoke job). + +Multicast CAN over loopback works on a stock VM with no route +configuration (the SITL self-tests its TX at startup). Timing notes for +slow or virtualised runners: the simulation paces itself and reports +the achieved ratio in `--verbose` (x1.00 = real time); the python test +senders keep their average frame rate under coarse sleep granularity by +sending catch-up bursts, which matters because the firmware's +bidirectional DShot auto-detect needs more than 100 frames before +zero-throttle arming completes, putting a floor of roughly 100Hz on the +usable frame rate. + +## Architecture + +The firmware runs unmodified (built as `am32_main()`) in one thread. A +simulation thread owns simulated time, advancing it in fixed physics +steps (500ns default) and delivering emulated interrupts (BEMF +comparator, commutation timer, 20kHz loop timer, CAN RX) by suspending +the firmware thread with a signal and running the handler, reproducing +the run-to-completion interrupt semantics of the real MCU. Simulated +time is decoupled from wall time and paced to `--speedup`. + +The emulated MCU follows the G431 target: TIM1 PWM generation with +preloaded ARR/CCR, a 2MHz interval timer, one-shot commutation timer, +20kHz loop timer and 1MHz utility timer, plus comparator/EXTI blanking +behaviour matching `Mcu/g431`. Timer reads from interrupt context step +the physics, so `delayMicros()` inside handlers and the comparator +filter loop behave as on hardware. + +The motor model (`sim/motor.c`) is a trapezoidal back-EMF BLDC model +based on open-bldc-csim: per-phase currents, solved star point voltage +(so the floating phase terminal voltage and its zero crossings are +physical), fet Rds_on, body diode clamping of a floating phase carrying +current, and battery voltage sag from internal resistance. The +comparator compares the floating phase against the virtual neutral with +configurable noise and hysteresis, so the firmware's blanking and +filtering logic is genuinely exercised at PWM switching level. diff --git a/Mcu/SITL/Src/ADC.c b/Mcu/SITL/Src/ADC.c new file mode 100644 index 000000000..a48f9efde --- /dev/null +++ b/Mcu/SITL/Src/ADC.c @@ -0,0 +1,37 @@ +/* + ADC.c - SITL. Converts the simulation sensor values into the raw ADC + counts the firmware expects, inverting the conversions in main.c + */ + +#include "ADC.h" + +#include "sitl.h" +#include "targets.h" + +extern uint16_t ADC_raw_temp; +extern uint16_t ADC_raw_volts; +extern uint16_t ADC_raw_current; + +void ADC_DMA_Callback(void) +{ + sitl_sensors_t s; + sitl_sensors_read(&s); + + // main.c: battery_voltage(10mV) = raw * 3300 / 4095 * VOLTAGE_DIVIDER / 100 + const float pin_mv_volts = s.bus_voltage * 1000.0f * 10.0f / TARGET_VOLTAGE_DIVIDER; + ADC_raw_volts = (uint16_t)(pin_mv_volts * 4095.0f / 3300.0f + 0.5f); + + // main.c: actual_current(10mA) = ((raw*3300/41) - CURRENT_OFFSET*100) / MILLIVOLT_PER_AMP + float pin_mv_current = s.bus_current * MILLIVOLT_PER_AMP + CURRENT_OFFSET; + if (pin_mv_current < 0) { + pin_mv_current = 0; + } + ADC_raw_current = (uint16_t)(pin_mv_current * 4095.0f / 3300.0f + 0.5f); + + // __LL_ADC_CALC_TEMPERATURE is a pass through in SITL + ADC_raw_temp = (uint16_t)(s.temperature_c + 0.5f); +} + +void ADC_Init(void) { } +void enableADC_DMA(void) { } +void activateADC(void) { } diff --git a/Mcu/SITL/Src/IO.c b/Mcu/SITL/Src/IO.c new file mode 100644 index 000000000..1e39bb742 --- /dev/null +++ b/Mcu/SITL/Src/IO.c @@ -0,0 +1,40 @@ +/* + IO.c - SITL signal IO. DShot/servo input arrives as UDP packets handled + by sitl_input.c, which emulates the input capture timer + DMA + */ + +#include "IO.h" + +#include "sitl.h" +#include "targets.h" + +uint32_t dma_buffer[64]; +volatile char out_put; +char ic_timer_prescaler = CPU_FREQUENCY_MHZ / 6; +uint8_t buffer_padding; + +void changeToOutput(void) { } +void changeToInput(void) { } + +void receiveDshotDma(void) +{ + out_put = 0; + sitl_input_arm(); +} + +void sendDshotDma(void) +{ + out_put = 1; + sitl_input_send_reply(); +} + +uint8_t getInputPinState(void) +{ + return sitl_input_pin_state(); +} + +void setInputPolarityRising(void) { } +void setInputPullDown(void) { } +void setInputPullUp(void) { } +void setInputPullNone(void) { } +void enableHalfTransferInt(void) { } diff --git a/Mcu/SITL/Src/comparator.c b/Mcu/SITL/Src/comparator.c new file mode 100644 index 000000000..6a08f29fc --- /dev/null +++ b/Mcu/SITL/Src/comparator.c @@ -0,0 +1,72 @@ +/* + comparator.c - SITL BEMF comparator emulation. Mirrors the g431 logic: + the floating phase is compared against the virtual neutral point, EXTI + edge selection follows `rising` (rising BEMF arms a falling edge on the + comparator output) + */ + +#include "comparator.h" + +#include "common.h" +#include "sitl.h" +#include "targets.h" + +sitl_exti_t sitl_exti; + +volatile uint8_t sitl_comp_phase = 2; +volatile uint8_t sitl_comp_out; + +COMP_TypeDef* active_COMP = &sitl_comp_dummy[1]; +uint32_t current_EXTI_LINE = SITL_EXTI_LINE_22; + +uint8_t getCompOutputLevel(void) +{ + if (sitl_in_sim_thread()) { + // called from interrupt context (filter loop in interruptRoutine): + // account for the read time so consecutive reads see fresh samples + sitl_isr_read_tick(); + } + return sitl_comp_out; +} + +void maskPhaseInterrupts(void) +{ + sitl_exti.IMR &= ~(SITL_EXTI_LINE_21 | SITL_EXTI_LINE_22); + sitl_exti.PR &= ~(SITL_EXTI_LINE_21 | SITL_EXTI_LINE_22); +} + +void enableCompInterrupts(void) +{ + sitl_exti.IMR |= current_EXTI_LINE; + // as on the STM32: an edge that latched PR while masked fires as + // soon as the interrupt is unmasked + if (sitl_exti.PR & current_EXTI_LINE) { + sitl_irq_pend(SITL_IRQ_COMP); + } +} + +void changeCompInput(void) +{ + if (step == 1 || step == 4) { // c floating + sitl_comp_phase = 2; + current_EXTI_LINE = SITL_EXTI_LINE_22; + active_COMP = &sitl_comp_dummy[1]; + } + if (step == 2 || step == 5) { // a floating + sitl_comp_phase = 0; + current_EXTI_LINE = SITL_EXTI_LINE_21; + active_COMP = &sitl_comp_dummy[0]; + } + if (step == 3 || step == 6) { // b floating + sitl_comp_phase = 1; + current_EXTI_LINE = SITL_EXTI_LINE_22; + active_COMP = &sitl_comp_dummy[1]; + } + if (rising) { + sitl_exti.RTSR &= ~(SITL_EXTI_LINE_21 | SITL_EXTI_LINE_22); + sitl_exti.FTSR |= current_EXTI_LINE; + } else { // falling bemf + sitl_exti.RTSR |= current_EXTI_LINE; + sitl_exti.FTSR &= ~(SITL_EXTI_LINE_21 | SITL_EXTI_LINE_22); + } +} diff --git a/Mcu/SITL/Src/eeprom.c b/Mcu/SITL/Src/eeprom.c new file mode 100644 index 000000000..75925c678 --- /dev/null +++ b/Mcu/SITL/Src/eeprom.c @@ -0,0 +1,67 @@ +/* + eeprom.c - SITL flash emulation backed by a file. The firmware reads and + writes 192 bytes at eeprom_address; the offset within the backing file is + relative to EEPROM_START_ADD + */ + +#include "eeprom.h" + +#include "sitl_config.h" +#include "targets.h" + +#include +#include + +void save_flash_nolib(uint8_t* data, int length, uint32_t add) +{ + const uint32_t offset = add - EEPROM_START_ADD; + FILE* f = fopen(sitl_cfg.eeprom_path, "r+b"); + if (!f) { + f = fopen(sitl_cfg.eeprom_path, "w+b"); + } + if (!f) { + perror("SITL: eeprom open"); + return; + } + fseek(f, offset, SEEK_SET); + fwrite(data, 1, length, f); + fclose(f); +} + +// provided by DroneCAN.c on CAN builds: the AM32 configurator default +// settings, used to seed a missing eeprom file so first boot behaves like +// a factory flashed ESC rather than erased flash +const uint8_t* DroneCAN_default_settings(unsigned* len) __attribute__((weak)); + +void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) +{ + const uint32_t offset = add - EEPROM_START_ADD; + // erased flash reads as 0xFF + memset(data, 0xFF, out_buff_len); + FILE* f = fopen(sitl_cfg.eeprom_path, "rb"); + if (!f) { + if (DroneCAN_default_settings != NULL && offset == 0) { + unsigned len = 0; + const uint8_t* def = DroneCAN_default_settings(&len); + if ((int)len > out_buff_len) { + len = out_buff_len; + } + memcpy(data, def, len); + if (out_buff_len > 27) { + // make the seeded settings describe the simulated motor, + // as a properly configured ESC would: a mismatched + // MOTOR_KV makes low rpm power protection clamp the duty + // at the wrong rpm. eeprom offsets 26/27 = motor_kv/poles, + // kv is stored as (kv-20)/40 + data[26] = (uint8_t)((sitl_cfg.motor.kv - 20.0f) / 40.0f + 0.5f); + data[27] = (uint8_t)sitl_cfg.motor.poles; + } + } + return; + } + fseek(f, offset, SEEK_SET); + if (fread(data, 1, out_buff_len, f) < (size_t)out_buff_len) { + // short file, rest stays 0xFF + } + fclose(f); +} diff --git a/Mcu/SITL/Src/peripherals.c b/Mcu/SITL/Src/peripherals.c new file mode 100644 index 000000000..25c3dd99d --- /dev/null +++ b/Mcu/SITL/Src/peripherals.c @@ -0,0 +1,122 @@ +/* + peripherals.c - SITL clock/peripheral bring-up, mirroring the g431 + version with the hardware replaced by the emulation + */ + +#include "peripherals.h" + +#include "ADC.h" +#include "comparator.h" +#include "sitl.h" +#include "targets.h" + +#include +#include + +GPIO_TypeDef sitl_gpio_dummy; +COMP_TypeDef sitl_comp_dummy[2]; + +void initAfterJump(void) { } + +void SystemClock_Config(void) { } +void MX_GPIO_Init(void) { } +void MX_DMA_Init(void) { } +void MX_COMP1_Init(void) { } +void MX_COMP2_Init(void) { } +void MX_TIM2_Init(void) { } +void MX_TIM3_Init(void) { } +void MX_TIM15_Init(void) { } +void MX_TIM17_Init(void) { } + +void MX_TIM1_Init(void) +{ + sitl_tim1_set_psc(0); + sitl_tim1_set_arr(TIM1_AUTORELOAD); + // dead time as on the g431 target; loadEEpromSettings ORs + // dead_time_override into BDTR on top of this + sitl_tim_deref(SITL_TIM1_IDX)->BDTR = DEAD_TIME; + sitl_tim1_force_update(); +} + +void MX_TIM6_Init(void) +{ + sitl_tim_deref(SITL_TIM6_IDX)->ARR = 1000000 / LOOP_FREQUENCY_HZ; +} + +void MX_TIM16_Init(void) { } + +void MX_IWDG_Init(void) +{ + sitl_watchdog_enable(); +} + +void initCorePeripherals(void) +{ + sitl_timers_init(); + SystemClock_Config(); + MX_GPIO_Init(); + MX_DMA_Init(); + MX_TIM1_Init(); + MX_TIM2_Init(); + MX_TIM6_Init(); + MX_TIM16_Init(); + MX_TIM17_Init(); + MX_COMP1_Init(); + MX_COMP2_Init(); +} + +void enableCorePeripherals(void) +{ + // default NVIC priorities matching the g431 target + sitl_nvic_set_priority(SITL_IRQ_COMP, 0); + sitl_nvic_set_priority(SITL_IRQ_COM, 0); + sitl_nvic_set_priority(SITL_IRQ_TENKHZ, 2); + sitl_nvic_set_priority(SITL_IRQ_DMA, 1); + sitl_nvic_set_priority(SITL_IRQ_EXTI15, 2); + sitl_nvic_set_priority(SITL_IRQ_CAN, 4); + + sitl_nvic_enable_irq(SITL_IRQ_COMP); + sitl_nvic_enable_irq(SITL_IRQ_COM); + sitl_nvic_enable_irq(SITL_IRQ_TENKHZ); + sitl_nvic_enable_irq(SITL_IRQ_DMA); + sitl_nvic_enable_irq(SITL_IRQ_EXTI15); + sitl_nvic_enable_irq(SITL_IRQ_CAN); + + sitl_tenkhz_enable(); +} + +void setPWMCompare1(uint16_t compareone) +{ + sitl_tim1_set_duty(0, compareone); +} + +void setPWMCompare2(uint16_t comparetwo) +{ + sitl_tim1_set_duty(1, comparetwo); +} + +void setPWMCompare3(uint16_t comparethree) +{ + sitl_tim1_set_duty(2, comparethree); +} + +void generatePwmTimerEvent(void) +{ + sitl_tim1_force_update(); +} + +void resetInputCaptureTimer(void) +{ + sitl_input_timer_reset(); +} + +void reloadWatchDogCounter(void) +{ + sitl_watchdog_reload(); +} + +void Error_Handler(void) +{ + fprintf(stderr, "SITL: Error_Handler called\n"); + exit(1); +} diff --git a/Mcu/SITL/Src/phaseouts.c b/Mcu/SITL/Src/phaseouts.c new file mode 100644 index 000000000..da862f207 --- /dev/null +++ b/Mcu/SITL/Src/phaseouts.c @@ -0,0 +1,114 @@ +/* + phaseouts.c - SITL bridge output control. Instead of GPIO mode changes + each phase gets a mode that the motor simulation samples together with + the emulated TIM1 to derive fet gate states + */ + +#include "phaseouts.h" + +#include "common.h" +#include "sitl.h" +#include "targets.h" + +volatile uint8_t sitl_phase_mode[3]; + +static void phasePWM(int p) +{ + if (!eepromBuffer.comp_pwm) { + sitl_phase_mode[p] = SITL_PHASE_PWM_NOCOMP; + } else { + sitl_phase_mode[p] = SITL_PHASE_PWM; + } +} + +static void phaseFLOAT(int p) +{ + sitl_phase_mode[p] = SITL_PHASE_FLOAT; +} + +static void phaseLOW(int p) +{ + sitl_phase_mode[p] = SITL_PHASE_LOW; +} + +void proportionalBrake(void) +{ + for (int p = 0; p < 3; p++) { + sitl_phase_mode[p] = SITL_PHASE_BRAKE_PWM; + } +} + +void allOff(void) +{ + phaseFLOAT(0); + phaseFLOAT(1); + phaseFLOAT(2); +} + +// commutation debug logging in the simulation +extern void motor_log_commutation(int step); + +void comStep(int newStep) +{ + motor_log_commutation(newStep); + switch (newStep) { + case 1: // A-B + phasePWM(0); + phaseLOW(1); + phaseFLOAT(2); + break; + case 2: // C-B + phaseFLOAT(0); + phaseLOW(1); + phasePWM(2); + break; + case 3: // C-A + phaseLOW(0); + phaseFLOAT(1); + phasePWM(2); + break; + case 4: // B-A + phaseLOW(0); + phasePWM(1); + phaseFLOAT(2); + break; + case 5: // B-C + phaseFLOAT(0); + phasePWM(1); + phaseLOW(2); + break; + case 6: // A-C + phasePWM(0); + phaseFLOAT(1); + phaseLOW(2); + break; + } +} + +void fullBrake(void) +{ + phaseLOW(0); + phaseLOW(1); + phaseLOW(2); +} + +void allpwm(void) +{ + phasePWM(0); + phasePWM(1); + phasePWM(2); +} + +void twoChannelForward(void) +{ + phasePWM(0); + phaseLOW(1); + phasePWM(2); +} + +void twoChannelReverse(void) +{ + phaseLOW(0); + phasePWM(1); + phaseLOW(2); +} diff --git a/Mcu/SITL/Src/serial_telemetry.c b/Mcu/SITL/Src/serial_telemetry.c new file mode 100644 index 000000000..fe198a81a --- /dev/null +++ b/Mcu/SITL/Src/serial_telemetry.c @@ -0,0 +1,12 @@ +/* + serial_telemetry.c - SITL stub + */ + +#include "serial_telemetry.h" + +void telem_UART_Init(void) { } + +void send_telem_DMA(uint8_t bytes) +{ + (void)bytes; +} diff --git a/Mcu/SITL/Src/sitl_compat.c b/Mcu/SITL/Src/sitl_compat.c new file mode 100644 index 000000000..e8ad24a7c --- /dev/null +++ b/Mcu/SITL/Src/sitl_compat.c @@ -0,0 +1,31 @@ +/* + sitl_compat.c - small portability helpers for non Linux hosts + */ + +#include "sitl.h" + +#include +#include +#include + +/* + a non blocking, close-on-exec UDP socket. macOS has no + SOCK_NONBLOCK/SOCK_CLOEXEC socket() flags + */ +int sitl_udp_socket(void) +{ +#ifdef __APPLE__ + const int fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd >= 0) { + fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK); + fcntl(fd, F_SETFD, FD_CLOEXEC); + // Without this, a failed send() on a connected UDP socket raises + // SIGPIPE (process exit -13) instead of returning -1/EPIPE. + const int one = 1; + setsockopt(fd, SOL_SOCKET, SO_NOSIGPIPE, &one, sizeof(one)); + } + return fd; +#else + return socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK | SOCK_CLOEXEC, 0); +#endif +} diff --git a/Mcu/SITL/Src/sitl_input.c b/Mcu/SITL/Src/sitl_input.c new file mode 100644 index 000000000..768ed5c82 --- /dev/null +++ b/Mcu/SITL/Src/sitl_input.c @@ -0,0 +1,348 @@ +/* + sitl_input.c - PWM/DShot signal input over UDP for the AM32 SITL + + Emulates the g431 input capture timer + DMA: each UDP packet carries one + frame on the virtual signal wire (a servo pulse or a complete 16 bit + DShot frame). Edge timestamps are synthesized in ticks of the emulated + capture timer and fed through the firmware's unmodified detection and + decode logic in Src/signal.c and Src/dshot.c, including input type + auto-detection, CRC checking and bidirectional DShot auto-detect. + + Bidirectional DShot replies (eRPM and extended telemetry) are decoded + from the firmware's GCR output buffer and sent back to the most recent + sender in the same packet format. + + packet format (little endian): + u16 magic 0x4453 + u8 type: 0=PWM, 1=DSHOT150, 2=DSHOT300, 3=DSHOT600 + u8 len: payload bytes after the 6 byte header (4) + u16 flags: bit0 = line idle level (1 = idle high, ie. inverted + bidirectional DShot) + u16 data: PWM pulse width in microseconds, or the full 16 bit DShot + frame (11 bit value, telemetry bit, 4 bit CRC) + */ + +#include "targets.h" +#include "common.h" +#include "dshot.h" +#include "IO.h" +#include "sitl.h" +#include "sitl_config.h" + +#include +#include +#include +#include +#include +#include +#include + +#define SITL_INPUT_MAGIC 0x4453 + +enum sitl_input_type { + SITL_INPUT_PWM = 0, + SITL_INPUT_DSHOT150 = 1, + SITL_INPUT_DSHOT300 = 2, + SITL_INPUT_DSHOT600 = 3, +}; + +#define SITL_INPUT_FLAG_IDLE_HIGH 0x0001 + +struct __attribute__((packed)) input_pkt { + uint16_t magic; + uint8_t type; + uint8_t len; + uint16_t flags; + uint16_t data; +}; + +extern void transfercomplete(void); +extern const char gcr_encode_table[16]; +extern volatile char out_put; + +static int fd = -1; +static struct sockaddr_in last_sender; +static bool have_sender; + +/* + virtual input capture peripheral, mirroring TIM15 + DMA on the g431: + reset-on-arm timer at 160MHz/(prescaler+1), capture values are the + timer count mod 65536 at each signal edge + */ +static volatile bool cap_armed; +static uint8_t cap_psc; // prescaler sampled at arm +static uint32_t cap_count; // DMA CNDTR equivalent +static uint32_t cap_index; +static uint64_t cap_base_ns; // sim time of CNT=0 + +static volatile uint8_t pin_idle_level; // from the last packet flags +static uint8_t last_type = SITL_INPUT_DSHOT300; +static uint64_t tx_done_ns; // sim time the BDShot reply transmit completes +static uint64_t dma_done_ns; // sim time RX capture DMA should complete (0 = idle) +static uint64_t last_edge_ns; // timestamp of the most recent synth edge + +// max UDP frames drained per poll so a flood cannot starve the sim thread +#define SITL_INPUT_DRAIN_MAX 32 + +static struct { + uint32_t frames; + uint32_t dropped; + uint32_t replies; + uint32_t bad_gcr; +} stats; + +void sitl_input_init(void) +{ + if (sitl_cfg.input_port <= 0) { + return; + } + fd = sitl_udp_socket(); + if (fd < 0) { + perror("SITL: input socket"); + return; + } + // no SO_REUSEADDR: a second instance on the same port must fail + // loudly instead of silently stealing datagrams + struct sockaddr_in addr; + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons((uint16_t)sitl_cfg.input_port); + addr.sin_addr.s_addr = htonl(sitl_cfg.bind_any ? INADDR_ANY : INADDR_LOOPBACK); + if (bind(fd, (struct sockaddr*)&addr, sizeof(addr)) != 0) { + perror("SITL: input bind"); + close(fd); + fd = -1; + return; + } + fprintf(stderr, "SITL: PWM/DShot input on udp port %d\n", sitl_cfg.input_port); +} + +// capture timer count at a simulated time, in ticks of 6.25ns*(psc+1) +static uint32_t cap_cnt_at(uint64_t t_ns) +{ + const uint64_t elapsed = t_ns - cap_base_ns; + return (uint32_t)((elapsed * 4U) / (25ULL * (cap_psc + 1U))) & 0xffffU; +} + +void sitl_input_arm(void) +{ + cap_armed = false; + cap_psc = (uint8_t)ic_timer_prescaler; + cap_count = buffersize; + cap_index = 0; + cap_base_ns = sitl_time_ns(); + cap_armed = true; +} + +void sitl_input_timer_reset(void) +{ + // resetInputCaptureTimer(): PSC=0, CNT=0, capture DMA untouched + cap_psc = 0; + cap_base_ns = sitl_time_ns(); +} + +uint8_t sitl_input_pin_state(void) +{ + return pin_idle_level; +} + +static void add_edge(uint64_t t_ns) +{ + if (cap_index < cap_count && cap_index < 64) { + dma_buffer[cap_index++] = cap_cnt_at(t_ns); + last_edge_ns = t_ns; + } +} + +static void synth_frame(uint8_t type, uint16_t data) +{ + const uint64_t now = sitl_time_ns(); + if (type == SITL_INPUT_PWM) { + // one pulse: rising then falling edge + add_edge(now); + add_edge(now + data * 1000ULL); + return; + } + uint32_t bit_ns; + switch (type) { + case SITL_INPUT_DSHOT150: + bit_ns = 6667; + break; + case SITL_INPUT_DSHOT300: + bit_ns = 3333; + break; + case SITL_INPUT_DSHOT600: + default: + bit_ns = 1667; + break; + } + // 16 bits MSB first, T1H = 0.75T, T0H = 0.375T. Polarity does not + // change the captured values with both-edge capture + for (int i = 0; i < 16; i++) { + const uint64_t start = now + (uint64_t)i * bit_ns; + const uint32_t high_ns = (data & (0x8000U >> i)) ? (bit_ns * 3U) / 4U : (bit_ns * 3U) / 8U; + add_edge(start); + add_edge(start + high_ns); + } +} + +// service deferred DMA completions (RX capture end / BDShot TX end) +static void service_deferred_dma(void) +{ + const uint64_t now = sitl_time_ns(); + if (out_put && tx_done_ns != 0 && now >= tx_done_ns) { + tx_done_ns = 0; + sitl_irq_pend(SITL_IRQ_DMA); + } + if (dma_done_ns != 0 && now >= dma_done_ns) { + dma_done_ns = 0; + sitl_irq_pend(SITL_IRQ_DMA); + } +} + +/* + decode the firmware's GCR output buffer back to the 16 bit BDShot reply + frame. gcr[bp] is the pre-transition idle level; the 21 bit line code is + gcr[bp+1..bp+21] (each entry 0 or 128, one output bit period each) and + the 20 GCR bits are the transitions between adjacent periods + */ +static bool decode_gcr(uint16_t* frame_out) +{ + uint32_t gcrnum = 0; + for (int j = 2; j <= 21; j++) { + const uint8_t bit = (gcr[buffer_padding + j] != 0) != (gcr[buffer_padding + j - 1] != 0); + gcrnum = (gcrnum << 1) | bit; + } + uint16_t frame = 0; + for (int q = 3; q >= 0; q--) { + const uint8_t code = (gcrnum >> (q * 5)) & 0x1f; + int nibble = -1; + for (int i = 0; i < 16; i++) { + if ((uint8_t)gcr_encode_table[i] == code) { + nibble = i; + break; + } + } + if (nibble < 0) { + return false; + } + frame = (frame << 4) | (uint16_t)nibble; + } + *frame_out = frame; + return true; +} + +void sitl_input_send_reply(void) +{ + uint16_t frame; + if (!decode_gcr(&frame)) { + stats.bad_gcr++; + frame = 0; + } else if (fd >= 0 && have_sender) { + struct input_pkt pkt = { + .magic = SITL_INPUT_MAGIC, + .type = last_type, + .len = 4, + // the BDShot reply line idles high, pulses are active low + .flags = SITL_INPUT_FLAG_IDLE_HIGH, + .data = frame, + }; + sendto(fd, &pkt, sizeof(pkt), 0, (struct sockaddr*)&last_sender, sizeof(last_sender)); + stats.replies++; + } + // the reply DMA completes after (23+buffer_padding) bit periods of + // 109 ticks at 160MHz/(output_timer_prescaler+1) + const uint64_t bit_ns = (109ULL * 25ULL * (uint64_t)(output_timer_prescaler + 1)) / 4ULL; + tx_done_ns = sitl_time_ns() + (23ULL + buffer_padding) * bit_ns; +} + +/* + DMA transfer complete interrupt, mirroring the g431 + DMA1_Channel1_IRQHandler. The servo half-transfer polarity flip is + deliberately collapsed: both edges of a pulse are captured at once + */ +void sitl_input_dma_irq(void) +{ + if (armed && dshot_telemetry) { + if (out_put) { + receiveDshotDma(); + compute_dshot_flag = 2; + } else { + sendDshotDma(); + compute_dshot_flag = 1; + } + sitl_irq_pend(SITL_IRQ_EXTI15); + return; + } + transfercomplete(); + sitl_irq_pend(SITL_IRQ_EXTI15); +} + +/* + called from the sim thread every physics step: service deferred DMA + IRQs, then pull UDP frames. DMA complete for a received frame is + deferred until the last synthesized edge time so frame duration is + visible in sim time. + + Wire-busy policy: + - BDShot TX or RX DMA still waiting for the last edge: stop receiving + and leave remaining datagrams queued. Consuming them would destroy + DShot command bursts (firmware needs 6 identical commands) that + GUI/catch-up senders place in the socket at once. + - Capture not armed: drain and drop (same as a real wire with no + DMA arm) so the socket does not accumulate stale zero-throttle + frames that would delay a later throttle change. + - After accepting a full capture: stop this poll so the rest of a + burst stays queued for the next free window. + */ +void sitl_input_poll(void) +{ + if (fd < 0) { + return; + } + service_deferred_dma(); + + for (int n = 0; n < SITL_INPUT_DRAIN_MAX; n++) { + // short busy windows: keep UDP queued for the next free step + if (out_put || dma_done_ns != 0) { + break; + } + struct input_pkt pkt; + struct sockaddr_in src; + socklen_t srclen = sizeof(src); + const ssize_t ret = recvfrom(fd, &pkt, sizeof(pkt), MSG_DONTWAIT, + (struct sockaddr*)&src, &srclen); + if (ret < 0) { + break; + } + if (ret < (ssize_t)sizeof(pkt) || pkt.magic != SITL_INPUT_MAGIC + || pkt.type > SITL_INPUT_DSHOT600 || pkt.len != 4) { + continue; + } + last_sender = src; + have_sender = true; + last_type = pkt.type; + pin_idle_level = (pkt.flags & SITL_INPUT_FLAG_IDLE_HIGH) ? 1 : 0; + stats.frames++; + if (!cap_armed) { + stats.dropped++; + continue; + } + synth_frame(pkt.type, pkt.data); + if (cap_index >= cap_count) { + cap_armed = false; + // complete at the last edge (or immediately if already past) + dma_done_ns = last_edge_ns ? last_edge_ns : sitl_time_ns(); + service_deferred_dma(); + break; + } + } +} + +void sitl_input_stats(uint32_t out[4]) +{ + out[0] = stats.frames; + out[1] = stats.dropped; + out[2] = stats.replies; + out[3] = stats.bad_gcr; +} diff --git a/Mcu/SITL/Src/sitl_it.c b/Mcu/SITL/Src/sitl_it.c new file mode 100644 index 000000000..099025ccf --- /dev/null +++ b/Mcu/SITL/Src/sitl_it.c @@ -0,0 +1,78 @@ +/* + sitl_it.c - the SITL "vector table", dispatching emulated interrupts to + the firmware handlers. Mirrors Mcu/g431/Src/stm32g4xx_it.c + */ + +#include "sitl.h" +#include "targets.h" + +extern void PeriodElapsedCallback(void); +extern void interruptRoutine(void); +extern void tenKhzRoutine(void); +extern void processDshot(void); + +// provided by sys_can_SITL.c when DroneCAN is compiled in +void sitl_can_irq(void) __attribute__((weak)); +void sitl_can_irq(void) { } + +// CAN statistics for --verbose, overridden by sys_can_SITL.c +void sitl_can_stats(uint32_t stats[4]) __attribute__((weak)); +void sitl_can_stats(uint32_t stats[4]) +{ + stats[0] = stats[1] = stats[2] = stats[3] = 0; +} + +extern volatile uint32_t commutation_interval; + +/* + comparator EXTI interrupt with the same blanking gate as the g431 + handler: ignore zero crossings in the first half of the expected + commutation interval + */ +extern void motor_log_event(int kind, uint32_t a, uint32_t b, uint32_t c); + +static void comp_irq(void) +{ + const uint32_t lines = SITL_EXTI_LINE_21 | SITL_EXTI_LINE_22; + const uint32_t cnt = sitl_interval_timer_count(); + if (cnt > (uint32_t)(commutation_interval >> 1)) { + if (sitl_exti.PR & lines) { + sitl_exti.PR &= ~lines; + motor_log_event(3 /*MEV_COMP_RUN*/, cnt, 0, 0); + interruptRoutine(); + } else { + // pend delivered with no pending line: the edge was consumed + // by an earlier handler + motor_log_event(3 /*MEV_COMP_RUN*/, cnt, 1, 0); + } + } else { + motor_log_event(2 /*MEV_COMP_BLANKED*/, cnt, commutation_interval >> 1, 0); + sitl_exti.PR &= ~lines; + } +} + +void sitl_irq_handler(int irq) +{ + switch (irq) { + case SITL_IRQ_COMP: + comp_irq(); + break; + case SITL_IRQ_COM: + PeriodElapsedCallback(); + break; + case SITL_IRQ_TENKHZ: + tenKhzRoutine(); + break; + case SITL_IRQ_DMA: + sitl_input_dma_irq(); + break; + case SITL_IRQ_EXTI15: + processDshot(); + break; + case SITL_IRQ_CAN: + sitl_can_irq(); + break; + default: + break; + } +} diff --git a/Mcu/SITL/Src/sitl_main.c b/Mcu/SITL/Src/sitl_main.c new file mode 100644 index 000000000..8fa03614d --- /dev/null +++ b/Mcu/SITL/Src/sitl_main.c @@ -0,0 +1,97 @@ +/* + sitl_main.c - entry point for the AM32 SITL build. Sets up the + simulation and then runs the firmware's main() (renamed to am32_main by + targets.h) + */ + +#include +#include +#include +#include +#include + +#include "sitl.h" +#include "sitl_config.h" +#include "motor.h" + +#include "eeprom.h" +#include "targets.h" + +// targets.h renames the firmware main() to am32_main; this file provides +// the real process main() +#undef main + +extern int am32_main(void); +extern void save_flash_nolib(uint8_t* data, int length, uint32_t add); +extern void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len); + +// force settings in the eeprom backing file from the command line +static void apply_eeprom_overrides(void) +{ + EEprom_t buf; + read_flash_bin(buf.buffer, EEPROM_START_ADD, sizeof(buf.buffer)); + bool changed = false; + if (sitl_cfg.node_id >= 0 && buf.can.can_node != (uint8_t)sitl_cfg.node_id) { + buf.can.can_node = (uint8_t)sitl_cfg.node_id; + changed = true; + } + if (sitl_cfg.input_type >= 0 && buf.input_type != (uint8_t)sitl_cfg.input_type) { + buf.input_type = (uint8_t)sitl_cfg.input_type; + changed = true; + } + if (changed) { + save_flash_nolib(buf.buffer, sizeof(buf.buffer), EEPROM_START_ADD); + } +} + +/* + hold an exclusive lock on the eeprom file for the life of the process. A + second instance sharing the eeprom (and therefore node ID) would corrupt + the DroneCAN traffic with interleaved transfers, which shows up as + erratic telemetry. The fd is CLOEXEC so a reset (re-exec) drops and + immediately re-acquires it + */ +static void lock_instance(void) +{ + // a separate lock file, so the eeprom file itself is only created by + // the firmware writing it (a missing eeprom triggers default seeding) + char lockpath[512]; + snprintf(lockpath, sizeof(lockpath), "%s.lock", sitl_cfg.eeprom_path); + const int fd = open(lockpath, O_RDWR | O_CREAT | O_CLOEXEC, 0644); + if (fd < 0) { + perror("SITL: lock file open"); + exit(1); + } + if (flock(fd, LOCK_EX | LOCK_NB) != 0) { + fprintf(stderr, + "SITL: %s is in use by another SITL instance. Use --eeprom and " + "--node-id to run multiple instances\n", + sitl_cfg.eeprom_path); + exit(1); + } + // fd deliberately left open to hold the lock +} + +int main(int argc, char** argv) +{ + // Connected UDP send() can raise SIGPIPE on macOS/BSD when multicast + // is unavailable (e.g. GitHub Actions runners). Ignore it so the + // caller gets EPIPE/ECONNREFUSED instead of a silent death. + signal(SIGPIPE, SIG_IGN); + + sitl_saved_argv = argv; + sitl_config_init(argc, argv); + lock_instance(); + motor_init(); + if (sitl_cfg.node_id >= 0 || sitl_cfg.input_type >= 0) { + apply_eeprom_overrides(); + } + + fprintf(stderr, "AM32 SITL: eeprom=%s can=%s speedup=%.1f\n", + sitl_cfg.eeprom_path, sitl_cfg.can_uri, (double)sitl_cfg.speedup); + + sitl_input_init(); + sitl_state_init(); + sitl_start_sim_thread(); + return am32_main(); +} diff --git a/Mcu/SITL/Src/sitl_sched.c b/Mcu/SITL/Src/sitl_sched.c new file mode 100644 index 000000000..15a941eb9 --- /dev/null +++ b/Mcu/SITL/Src/sitl_sched.c @@ -0,0 +1,568 @@ +/* + sitl_sched.c - simulated time, interrupt delivery and pacing for AM32 SITL + + The sim thread advances simulated time in fixed physics steps. Emulated + interrupts are delivered by suspending the firmware thread with SIGUSR1 + (parking it on a semaphore) and running the handler in the sim thread, + which reproduces the run-to-completion, mainline-frozen semantics of real + interrupts. __disable_irq()/__enable_irq() map onto an atomic PRIMASK + flag; while set, events stay pending exactly as on hardware. + */ + +#include "sitl.h" +#include "sitl_config.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef __linux__ +#include +#endif +#include +#include + +#include "motor.h" + +static volatile uint64_t sim_time_ns_v; +static pthread_t sim_thread_id; +static pthread_t fw_thread_id; + +static volatile int primask; // 1 = interrupts disabled, atomic stores only + +// simulated time granted by firmware-thread timer reads while it holds +// PRIMASK (a startup tune busy-waits on the utility timer under +// __disable_irq, so time must still advance for it). Reset when the +// critical section ends +static volatile uint64_t fw_grant_ns; +static volatile uint32_t irq_pending; +static volatile uint32_t irq_enabled; +static volatile uint8_t irq_prio[SITL_IRQ_MAX]; + + +/* + park/resume semaphores. macOS has no unnamed POSIX semaphores or + sem_timedwait, so it uses GCD semaphores instead + */ +#ifdef __APPLE__ +#include +typedef dispatch_semaphore_t sitl_sem_t; +static void sitl_sem_init(sitl_sem_t* s) { *s = dispatch_semaphore_create(0); } +static void sitl_sem_post(sitl_sem_t* s) { dispatch_semaphore_signal(*s); } +static void sitl_sem_wait(sitl_sem_t* s) +{ + dispatch_semaphore_wait(*s, DISPATCH_TIME_FOREVER); +} +static bool sitl_sem_wait_2s(sitl_sem_t* s) +{ + return dispatch_semaphore_wait(*s, dispatch_time(DISPATCH_TIME_NOW, 2LL * NSEC_PER_SEC)) == 0; +} +#else +typedef sem_t sitl_sem_t; +static void sitl_sem_init(sitl_sem_t* s) { sem_init(s, 0, 0); } +static void sitl_sem_post(sitl_sem_t* s) { sem_post(s); } +static void sitl_sem_wait(sitl_sem_t* s) +{ + while (sem_wait(s) == -1 && errno == EINTR) { + } +} +static bool sitl_sem_wait_2s(sitl_sem_t* s) +{ + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += 2; + for (;;) { + if (sem_timedwait(s, &ts) == 0) { + return true; + } + if (errno != EINTR) { + return false; + } + } +} +#endif + +static sitl_sem_t park_sem, resume_sem; + +static volatile uint64_t watchdog_last_reload_ns; +static volatile bool watchdog_running; +#define WATCHDOG_TIMEOUT_NS 2000000000ULL + +char** sitl_saved_argv; + +// implemented in sitl_it.c +extern void sitl_irq_handler(int irq); +// implemented in sys_can_SITL.c when DroneCAN is compiled in +void sitl_can_poll(void) __attribute__((weak)); +void sitl_can_poll(void) { } + +uint64_t sitl_time_ns(void) +{ + return sim_time_ns_v; +} + +bool sitl_in_sim_thread(void) +{ + return pthread_equal(pthread_self(), sim_thread_id); +} + +static uint64_t wallclock_ns(void) +{ + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return (uint64_t)ts.tv_sec * 1000000000ULL + ts.tv_nsec; +} + +/* + NVIC emulation + */ +void sitl_nvic_set_priority(int irq, uint32_t prio) +{ + irq_prio[irq] = prio; +} + +void sitl_nvic_enable_irq(int irq) +{ + __atomic_fetch_or(&irq_enabled, 1U << irq, __ATOMIC_SEQ_CST); +} + +void sitl_nvic_disable_irq(int irq) +{ + __atomic_fetch_and(&irq_enabled, ~(1U << irq), __ATOMIC_SEQ_CST); +} + +/* + dispatch block diagnostic: records why a pending COMP/COM interrupt is + not being delivered, classified per simulation step, reported when the + delivery latency exceeds 20us (bounded number of reports) + */ +static uint64_t irq_pend_ns[SITL_IRQ_MAX]; +static int current_irq = -1; +static struct { + uint32_t steps_primask; + uint32_t steps_active[SITL_IRQ_MAX]; + uint32_t steps_disabled; + uint32_t steps_free; +} blocked; + +void sitl_irq_pend(int irq) +{ + const uint32_t old = __atomic_fetch_or(&irq_pending, 1U << irq, __ATOMIC_SEQ_CST); + if ((old & (1U << irq)) == 0) { + irq_pend_ns[irq] = sim_time_ns_v; + } +} + +void sitl_primask_set(void) +{ + // plain atomic store: the dispatcher re-checks primask after parking + // the firmware thread, so no lock is needed. Critical sections are + // extremely frequent (micros64 runs one per call) and must be cheap + __atomic_store_n(&primask, 1, __ATOMIC_SEQ_CST); +} + +uint32_t sitl_primask_get(void) +{ + return (uint32_t)__atomic_load_n(&primask, __ATOMIC_SEQ_CST); +} + +void sitl_primask_clear(void) +{ + if (!sitl_in_sim_thread()) { + // grants are per critical section: unconsumed ones must not + // accumulate into a reservoir that lets the simulation run + // through a later, host-stretched critical section + __atomic_store_n(&fw_grant_ns, 0, __ATOMIC_SEQ_CST); + } + __atomic_store_n(&primask, 0, __ATOMIC_SEQ_CST); +} + +/* + firmware thread suspension. SIGUSR1 parks the firmware thread on + resume_sem. Blocking in a handler is not formally async-signal-safe, + but these are bare syscall wrappers taking no library locks; the one + real hazard is parking this thread inside a locked stdio call while + the sim thread also prints, so diagnostics prints are kept rare + */ +static void sigusr1_handler(int sig) +{ + (void)sig; + const int saved_errno = errno; + sitl_sem_post(&park_sem); + sitl_sem_wait(&resume_sem); + errno = saved_errno; +} + +static bool suspend_firmware(void) +{ + pthread_kill(fw_thread_id, SIGUSR1); + // a timed wait so a firmware thread that is exiting or execing + // (NVIC_SystemReset) cannot deadlock the simulation + return sitl_sem_wait_2s(&park_sem); +} + +static void resume_firmware(void) +{ + sitl_sem_post(&resume_sem); +} + +// priority of the currently executing handler, NVIC style (lower value +// is higher priority). 1000 = thread level, nothing active +static int active_irq_prio = 1000; + + +// run any pending interrupt with higher priority than the active one. +// Called from the dispatch loop and re-entrantly from sitl_isr_read_tick +// so a long running handler (eg tenKhzRoutine busy waiting on a timer) +// can be preempted by the comparator, as the NVIC does on real hardware +static void run_pending_irqs(void) +{ + for (;;) { + if (primask) { + return; + } + const uint32_t active = irq_pending & irq_enabled; + if (active == 0) { + return; + } + int best = -1; + for (int irq = 0; irq < SITL_IRQ_MAX; irq++) { + if ((active & (1U << irq)) == 0) { + continue; + } + if (best < 0 || irq_prio[irq] < irq_prio[best]) { + best = irq; + } + } + if (irq_prio[best] >= active_irq_prio) { + // equal or lower priority does not preempt + return; + } + __atomic_fetch_and(&irq_pending, ~(1U << best), __ATOMIC_SEQ_CST); + if (best == SITL_IRQ_COMP || best == SITL_IRQ_COM) { + const uint64_t lat = sim_time_ns_v - irq_pend_ns[best]; + static int prints; + if (lat > 20000 && prints < 12) { + prints++; + fprintf(stderr, + "SITL: irq %d blocked %.1fus at t=%.4f: primask=%u dis=%u free=%u" + " act=[%u,%u,%u,%u,%u,%u]\n", + best, lat * 1e-3, sim_time_ns_v * 1e-9, + blocked.steps_primask, blocked.steps_disabled, blocked.steps_free, + blocked.steps_active[0], blocked.steps_active[1], + blocked.steps_active[2], blocked.steps_active[3], + blocked.steps_active[4], blocked.steps_active[5]); + } + memset(&blocked, 0, sizeof(blocked)); + } + const int saved_prio = active_irq_prio; + const int saved_irq = current_irq; + active_irq_prio = irq_prio[best]; + current_irq = best; + sitl_irq_handler(best); + current_irq = saved_irq; + active_irq_prio = saved_prio; + } +} + +/* + deliver pending enabled interrupts, called from the sim thread between + physics steps + */ +static void sitl_dispatch(void) +{ + if ((irq_pending & irq_enabled) == 0 || primask) { + return; + } + if (!suspend_firmware()) { + return; + } + // the firmware may have entered a critical section between our check + // and it parking; if so it is now parked inside the section and we + // must not run handlers (an IRQ arriving just after cpsid is held + // pending on real hardware too) + if (!primask) { + run_pending_irqs(); + } + resume_firmware(); +} + +/* + watchdog + */ +void sitl_watchdog_reload(void) +{ + extern void motor_log_mainloop(void); + motor_log_mainloop(); + watchdog_last_reload_ns = sim_time_ns_v; + if (!sitl_in_sim_thread() && sitl_cfg.sim.loop_time_ns > 0 && sitl_cfg.speedup > 0) { + // approximate the real main loop execution time so the firmware + // thread does not spin flat out + const uint64_t delay_ns = (uint64_t)(sitl_cfg.sim.loop_time_ns / sitl_cfg.speedup); + if (sitl_cfg.nosleep) { + const uint64_t deadline = wallclock_ns() + delay_ns; + while (wallclock_ns() < deadline) { + } + } else { + struct timespec ts = { 0, (long)delay_ns }; + nanosleep(&ts, NULL); + } + } +} + +void sitl_watchdog_enable(void) +{ + watchdog_last_reload_ns = sim_time_ns_v; + watchdog_running = sitl_cfg.sim.watchdog_enabled; +} + +static void watchdog_check(void) +{ + if (watchdog_running && sim_time_ns_v - watchdog_last_reload_ns > WATCHDOG_TIMEOUT_NS) { + fprintf(stderr, "SITL: watchdog reset at t=%.3fs\n", sim_time_ns_v * 1.0e-9); + sitl_system_reset(); + } +} + +void sitl_system_reset(void) +{ + // block the suspension signal so a concurrent interrupt delivery + // cannot park this thread on the way into exec + sigset_t set; + sigemptyset(&set); + sigaddset(&set, SIGUSR1); + pthread_sigmask(SIG_BLOCK, &set, NULL); + fprintf(stderr, "SITL: reset at t=%.3fs\n", sim_time_ns_v * 1.0e-9); +#ifdef __APPLE__ + execv(sitl_saved_argv[0], sitl_saved_argv); +#else + execv("/proc/self/exe", sitl_saved_argv); +#endif + fprintf(stderr, "SITL: execv failed: %s\n", strerror(errno)); + _exit(1); +} + +/* + advance simulation by one physics step. Called from the sim thread main + loop and re-entrantly from interrupt handlers that busy wait on time + (delayMicros, comparator filter reads) + */ +static void sim_step_once(void) +{ + const uint32_t dt = sitl_cfg.sim.physics_dt_ns; + motor_step(sim_time_ns_v, dt); + sim_time_ns_v += dt; + sitl_timers_step(sim_time_ns_v); + sitl_state_step(sim_time_ns_v); + + // dispatch block tracer: classify this step if a priority 0 interrupt + // has been pending for a while + for (int irq = SITL_IRQ_COMP; irq <= SITL_IRQ_COM; irq++) { + if ((irq_pending & (1U << irq)) == 0) { + continue; + } + if (sim_time_ns_v - irq_pend_ns[irq] <= 20000) { + continue; + } + if (current_irq >= 0) { + // a handler is executing (possibly holding primask itself) + blocked.steps_active[current_irq]++; + } else if (primask) { + blocked.steps_primask++; + } else if ((irq_enabled & (1U << irq)) == 0) { + blocked.steps_disabled++; + } else { + blocked.steps_free++; + } + break; + } +} + +void sitl_step_from_isr(void) +{ + sim_step_once(); +} + +void sitl_fw_read_tick(void) +{ + if (primask && !sitl_in_sim_thread()) { + __atomic_fetch_add(&fw_grant_ns, sitl_cfg.sim.isr_read_ns, __ATOMIC_SEQ_CST); + } +} + +void sitl_isr_read_tick(void) +{ + // only called from the sim thread (interrupt context), no locking + // needed. Advancing a full physics step per register read would make + // handler busy loops take ~10x longer in simulated time than on real + // hardware, which breaks the comparator filter in interruptRoutine() + static uint32_t accum_ns; + accum_ns += sitl_cfg.sim.isr_read_ns; + while (accum_ns >= sitl_cfg.sim.physics_dt_ns) { + accum_ns -= sitl_cfg.sim.physics_dt_ns; + sim_step_once(); + // let higher priority interrupts preempt the current handler + run_pending_irqs(); + } +} + +// try to switch the calling thread to SCHED_FIFO, warning on failure +static void set_realtime(const char* what) +{ + if (!sitl_cfg.realtime) { + return; + } + struct sched_param sp = { .sched_priority = 50 }; + if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sp) != 0) { + fprintf(stderr, "SITL: SCHED_FIFO failed for %s thread: %s\n", + what, strerror(errno)); + return; + } + // with --nosleep the threads never block, so default RT throttling + // (kernel.sched_rt_runtime_us=950000) would stall them 50ms/second + if (sitl_cfg.nosleep) { + FILE* f = fopen("/proc/sys/kernel/sched_rt_runtime_us", "r"); + if (f) { + long v = 0; + if (fscanf(f, "%ld", &v) == 1 && v != -1) { + fprintf(stderr, + "SITL: WARNING: RT throttling is enabled and will stall " + "--nosleep --realtime; run " + "'sysctl -w kernel.sched_rt_runtime_us=-1'\n"); + } + fclose(f); + } + } + fprintf(stderr, "SITL: %s thread using SCHED_FIFO\n", what); +} + +static void* sim_thread_main(void* arg) +{ + (void)arg; + set_realtime("sim"); +#ifdef __linux__ + // timer slack defaults to 50us which ruins short pacing sleeps + prctl(PR_SET_TIMERSLACK, 1UL); +#endif + const uint64_t wall0 = wallclock_ns(); + uint64_t next_can_poll_ns = 0; + uint64_t next_pace_check_ns = 0; + uint64_t verbose_last_ns = 0; + uint64_t verbose_last_wall = wall0; + uint64_t pace_wall_ref = wall0; + uint64_t pace_sim_ref = 0; + float pace_speedup = sitl_cfg.speedup; + + uint32_t grant_accum_ns = 0; + for (;;) { + /* + while the firmware thread holds PRIMASK outside of interrupt + context, simulated time may only advance as granted by the + firmware's own timer reads. On the real MCU a critical section + lasts nanoseconds; if the host deschedules the firmware thread + inside one (OS preemption, hard/soft irqs on its core), a free + running clock would block interrupt delivery for hundreds of + microseconds of simulated time and lose commutation timing. + current_irq is only ever set by this thread, so the check does + not race: ISR-context PRIMASK always has current_irq >= 0 + */ + if (primask && current_irq < 0) { + const uint64_t g = fw_grant_ns; + if (g > 0) { + __atomic_fetch_sub(&fw_grant_ns, g, __ATOMIC_SEQ_CST); + grant_accum_ns += (uint32_t)g; + } + if (grant_accum_ns < sitl_cfg.sim.physics_dt_ns) { + continue; + } + grant_accum_ns -= sitl_cfg.sim.physics_dt_ns; + } else { + grant_accum_ns = 0; + } + sim_step_once(); + const uint64_t now = sim_time_ns_v; + + // input every physics step so deferred DMA matches frame edges and + // UDP bursts drain without waiting for the 100us CAN/state cadence + sitl_input_poll(); + if (now >= next_can_poll_ns) { + next_can_poll_ns = now + 100000; // 100us + sitl_can_poll(); + sitl_state_poll(); + } + watchdog_check(); + sitl_dispatch(); + + // pace simulated time against the wall clock, sleeping to an + // absolute deadline so overshoot does not accumulate. The + // references rebase when the speedup changes at runtime (GUI + // slow motion control) so the mapping stays continuous + if (sitl_cfg.speedup != pace_speedup) { + pace_speedup = sitl_cfg.speedup; + pace_wall_ref = wallclock_ns(); + pace_sim_ref = now; + } + if (pace_speedup > 0 && now >= next_pace_check_ns) { + next_pace_check_ns = now + 50000; // check every 50us of sim time + const uint64_t target_wall = pace_wall_ref + (uint64_t)((double)(now - pace_sim_ref) / pace_speedup); + const uint64_t wall = wallclock_ns(); + if (sitl_cfg.nosleep) { + while (wallclock_ns() < target_wall) { + } + } else if (target_wall > wall + 100000) { +#ifdef __APPLE__ + // no clock_nanosleep on macOS: relative sleep. Drift free + // because the absolute target is recomputed each pass + const uint64_t delta = target_wall - wall; + struct timespec ts = { delta / 1000000000ULL, delta % 1000000000ULL }; + nanosleep(&ts, NULL); +#else + struct timespec ts = { target_wall / 1000000000ULL, target_wall % 1000000000ULL }; + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, NULL); +#endif + } + } + + if (sitl_cfg.verbose && now - verbose_last_ns >= 1000000000ULL) { + const uint64_t wall = wallclock_ns(); + const float ratio = (float)(now - verbose_last_ns) / (float)(wall - verbose_last_wall); + verbose_last_ns = now; + verbose_last_wall = wall; + motor_print_state(now, ratio); + } + } + return NULL; +} + +void sitl_start_sim_thread(void) +{ + fw_thread_id = pthread_self(); + set_realtime("firmware"); +#ifdef __linux__ + // timer slack is per thread and defaults to 50us + prctl(PR_SET_TIMERSLACK, 1UL); +#endif + sitl_sem_init(&park_sem); + sitl_sem_init(&resume_sem); + + // sitl_system_reset blocks SIGUSR1 on the way into execv; both the + // mask and a possibly pending SIGUSR1 survive exec. Discard any + // pending instance and unblock before installing the real handler + signal(SIGUSR1, SIG_IGN); + sigset_t set; + sigemptyset(&set); + sigaddset(&set, SIGUSR1); + pthread_sigmask(SIG_UNBLOCK, &set, NULL); + + struct sigaction sa; + memset(&sa, 0, sizeof(sa)); + sa.sa_handler = sigusr1_handler; + sa.sa_flags = SA_RESTART; + sigaction(SIGUSR1, &sa, NULL); + + pthread_create(&sim_thread_id, NULL, sim_thread_main, NULL); +} diff --git a/Mcu/SITL/Src/sitl_state.c b/Mcu/SITL/Src/sitl_state.c new file mode 100644 index 000000000..481db9599 --- /dev/null +++ b/Mcu/SITL/Src/sitl_state.c @@ -0,0 +1,258 @@ +/* + sitl_state.c - high rate simulation state streaming and runtime model + control over UDP, for GUI graphs and animations + + A client subscribes by sending a SUBSCRIBE packet with the desired + sample period in simulated nanoseconds; the simulation thread then + streams batched state samples (rotor angle/speed, phase currents, bus + voltage/current, bridge modes, comparator) to the subscriber. The + subscription expires two seconds after the last refresh. + + A LOAD_MODEL packet carries the path of a motor/battery/esc JSON file + (same format as --config, sim section ignored) which is applied to the + running simulation. + + client -> SITL (little endian): + u16 magic 0x5353, u8 cmd, u8 pad, payload + cmd 0 SUBSCRIBE: u32 period_ns; flags byte bit0 = averaged + sampling (currents/voltages are the mean over each sample + period instead of instantaneous, avoiding PWM aliasing at + coarse periods) + cmd 1 LOAD_MODEL: JSON file path (rest of packet) + cmd 2 SET_SPEEDUP: float speedup (0 = free run) + SITL -> client: + u16 magic 0x5354, u8 version=1, u8 count, count * sample + u16 magic 0x5355, u8 ok, u8 pad, message (LOAD_MODEL reply) +*/ + +#include "sitl.h" +#include "sitl_config.h" +#include "motor.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#define STATE_MAGIC_CMD 0x5353 +#define STATE_MAGIC_DATA 0x5354 +#define STATE_MAGIC_REPLY 0x5355 + +struct __attribute__((packed)) state_sample { + uint64_t t_ns; + float omega; // mechanical rad/s + float theta; // mechanical angle, rad [0,2pi) + float theta_e; // electrical angle, rad [0,2pi) + float iu, iv, iw; // phase currents, A + float vu, vv, vw; // phase terminal voltages, V + float vbus, ibus; + uint8_t modes[3]; // sitl_phase_mode per phase + uint8_t comp_phase; // floating phase + uint8_t comp_out; + uint8_t pad[3]; +}; + +#define STATE_BATCH 16 + +static int fd = -1; +static struct sockaddr_in sub_addr; +static bool have_sub; +static time_t sub_expire; +static uint32_t period_req_ns = 50000; // requested by the subscriber +static uint32_t period_ns = 50000; // effective, wall rate limited +static bool averaged; // mean over the period instead of point samples +static double sig_acc[8]; +static uint32_t sig_n; +static uint64_t next_sample_ns; +static uint64_t last_flush_ns; + +static struct __attribute__((packed)) { + uint16_t magic; + uint8_t version; + uint8_t count; + struct state_sample s[STATE_BATCH]; +} batch = { .magic = STATE_MAGIC_DATA, .version = 2 }; + +void sitl_state_init(void) +{ + if (sitl_cfg.state_port <= 0) { + return; + } + fd = sitl_udp_socket(); + if (fd < 0) { + perror("SITL: state socket"); + return; + } + // no SO_REUSEADDR: a second instance on the same port must fail + // loudly instead of silently stealing datagrams + struct sockaddr_in addr; + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons((uint16_t)sitl_cfg.state_port); + addr.sin_addr.s_addr = htonl(sitl_cfg.bind_any ? INADDR_ANY : INADDR_LOOPBACK); + if (bind(fd, (struct sockaddr*)&addr, sizeof(addr)) != 0) { + perror("SITL: state bind"); + close(fd); + fd = -1; + return; + } + fprintf(stderr, "SITL: state/model port udp %d\n", sitl_cfg.state_port); +} + +static void load_model(const char* path, struct sockaddr_in* src) +{ + char msg[256]; + const bool ok = sitl_config_reload(path); + if (ok) { + motor_config_changed(); + snprintf(msg, sizeof(msg), "loaded %.200s", path); + fprintf(stderr, "SITL: %s\n", msg); + } else { + snprintf(msg, sizeof(msg), "failed to load %.200s", path); + fprintf(stderr, "SITL: %s\n", msg); + } + struct __attribute__((packed)) { + uint16_t magic; + uint8_t ok; + uint8_t pad; + char msg[256]; + } reply = { .magic = STATE_MAGIC_REPLY, .ok = ok, .pad = 0 }; + strncpy(reply.msg, msg, sizeof(reply.msg) - 1); + sendto(fd, &reply, 4 + strlen(reply.msg) + 1, 0, (struct sockaddr*)src, sizeof(*src)); +} + +/* + effective sample period: the subscriber's request, floored to the + physics step and rate limited to about 200k samples per second of + wall clock so fine sampling does not overload the sim thread at high + speedups. Slowing the simulation down automatically allows finer + sampling + */ +static void apply_period(void) +{ + uint32_t period = period_req_ns; + if (period < sitl_cfg.sim.physics_dt_ns) { + period = sitl_cfg.sim.physics_dt_ns; + } + const uint32_t wall_floor = (uint32_t)(5000.0f * sitl_cfg.speedup); + if (sitl_cfg.speedup > 0 && period < wall_floor) { + period = wall_floor; + } + period_ns = period; +} + +// called from the sim thread every 100us +void sitl_state_poll(void) +{ + if (fd < 0) { + return; + } + if (have_sub && time(NULL) > sub_expire) { + have_sub = false; + } + uint8_t pkt[512]; + struct sockaddr_in src; + socklen_t srclen = sizeof(src); + const ssize_t ret = recvfrom(fd, pkt, sizeof(pkt) - 1, MSG_DONTWAIT, (struct sockaddr*)&src, &srclen); + if (ret < 4) { + return; + } + uint16_t magic; + memcpy(&magic, pkt, 2); + if (magic != STATE_MAGIC_CMD) { + return; + } + const uint8_t cmd = pkt[2]; + if (cmd == 0 && ret >= 8) { + memcpy(&period_req_ns, pkt + 4, 4); + averaged = (pkt[3] & 1) != 0; + apply_period(); + // a new subscriber must not receive samples batched for the + // previous one + if (!have_sub || src.sin_addr.s_addr != sub_addr.sin_addr.s_addr + || src.sin_port != sub_addr.sin_port) { + batch.count = 0; + memset(sig_acc, 0, sizeof(sig_acc)); + sig_n = 0; + next_sample_ns = 0; + } + sub_addr = src; + have_sub = true; + sub_expire = time(NULL) + 2; + } else if (cmd == 1) { + pkt[ret] = 0; + load_model((const char*)(pkt + 4), &src); + } else if (cmd == 2 && ret >= 8) { + float speedup; + memcpy(&speedup, pkt + 4, 4); + if (speedup >= 0 && speedup <= 100) { + // the pacing loop rebases its references on change + sitl_cfg.speedup = speedup; + apply_period(); + fprintf(stderr, "SITL: speedup %.3f\n", (double)speedup); + } + } +} + +// called from the sim thread on every physics step +void sitl_state_step(uint64_t now_ns) +{ + if (!have_sub) { + return; + } + if (averaged) { + motor_add_signals(sig_acc); + sig_n++; + } + if (now_ns < next_sample_ns) { + return; + } + next_sample_ns = now_ns + period_ns; + + struct state_sample* s = &batch.s[batch.count]; + memset(s, 0, sizeof(*s)); + s->t_ns = now_ns; + float omega, theta, theta_e, i[3], v[3], vbus, ibus; + motor_get_live_state(&omega, &theta, &theta_e, i, v, &vbus, &ibus); + s->omega = omega; + s->theta = theta; + s->theta_e = theta_e; + if (averaged && sig_n > 0) { + s->iu = (float)(sig_acc[0] / sig_n); + s->iv = (float)(sig_acc[1] / sig_n); + s->iw = (float)(sig_acc[2] / sig_n); + s->vu = (float)(sig_acc[3] / sig_n); + s->vv = (float)(sig_acc[4] / sig_n); + s->vw = (float)(sig_acc[5] / sig_n); + s->vbus = (float)(sig_acc[6] / sig_n); + s->ibus = (float)(sig_acc[7] / sig_n); + memset(sig_acc, 0, sizeof(sig_acc)); + sig_n = 0; + } else { + s->iu = i[0]; + s->iv = i[1]; + s->iw = i[2]; + s->vu = v[0]; + s->vv = v[1]; + s->vw = v[2]; + s->vbus = vbus; + s->ibus = ibus; + } + for (int p = 0; p < 3; p++) { + s->modes[p] = sitl_phase_mode[p]; + } + s->comp_phase = sitl_comp_phase; + s->comp_out = sitl_comp_out; + batch.count++; + + if (batch.count >= STATE_BATCH || now_ns - last_flush_ns > 5000000ULL) { + sendto(fd, &batch, 4 + batch.count * sizeof(struct state_sample), 0, + (struct sockaddr*)&sub_addr, sizeof(sub_addr)); + batch.count = 0; + last_flush_ns = now_ns; + } +} diff --git a/Mcu/SITL/Src/sitl_timers.c b/Mcu/SITL/Src/sitl_timers.c new file mode 100644 index 000000000..3632d0d7d --- /dev/null +++ b/Mcu/SITL/Src/sitl_timers.c @@ -0,0 +1,263 @@ +/* + sitl_timers.c - emulation of the timers AM32 uses on a G431: + TIM1 PWM generation, 160MHz, ARR/CCR preloaded + TIM2 INTERVAL_TIMER, 2MHz, 32 bit free running + TIM6 TEN_KHZ_TIMER, periodic update interrupt (20kHz loop) + TIM16 COM_TIMER, 2MHz, one-shot style update interrupt + TIM17 UTILITY_TIMER, 1MHz, 16 bit free running + + Counters are derived from simulated time. Field writes from the firmware + thread are plain volatile stores; consistency relies on x86 store + ordering plus enable-last write order in the arm helpers. + */ + +#include "sitl.h" +#include + +static SITL_TIM_TypeDef tims[SITL_NUM_TIMS]; + +// nanoseconds per CPU clock tick numerator/denominator: 160MHz = 6.25ns +#define CPU_TICK_NS_NUM 25 +#define CPU_TICK_NS_DEN 4 + +static struct { + // active (shadow) registers, only updated at update events + uint32_t psc_act, arr_act, ccr_act[3]; + // preload values written by the firmware + volatile uint32_t psc_pre, arr_pre, ccr_pre[3]; + volatile uint64_t period_start_ns; +} tim1; + +static struct { + volatile uint64_t base_ns; + volatile uint32_t cnt_base; +} tim2, tim16; + +static struct { + volatile uint64_t next_due_ns; + volatile bool enabled; +} tim6; + +static uint64_t tim1_period_ns(void) +{ + const uint64_t ticks = (uint64_t)(tim1.psc_act + 1) * (tim1.arr_act + 1); + return (ticks * CPU_TICK_NS_NUM) / CPU_TICK_NS_DEN; +} + +static void tim1_latch(void) +{ + tim1.psc_act = tim1.psc_pre; + tim1.arr_act = tim1.arr_pre; + for (int i = 0; i < 3; i++) { + tim1.ccr_act[i] = tim1.ccr_pre[i]; + } +} + +void sitl_timers_init(void) +{ + tim1.psc_pre = 0; + tim1.arr_pre = 1999; // overwritten by MX_TIM1_Init + for (int i = 0; i < 3; i++) { + tim1.ccr_pre[i] = 0; + } + tim1_latch(); + tim1.period_start_ns = 0; + tims[SITL_TIM6_IDX].ARR = 50; +} + +// current TIM1 counter position in timer ticks +static uint32_t tim1_cnt(uint64_t now_ns) +{ + const uint64_t elapsed = now_ns - tim1.period_start_ns; + const uint64_t tick_ns_num = (uint64_t)CPU_TICK_NS_NUM * (tim1.psc_act + 1); + // elapsed ticks = elapsed_ns * DEN / (NUM*(psc+1)) + return (uint32_t)((elapsed * CPU_TICK_NS_DEN) / tick_ns_num); +} + +bool sitl_tim1_pwm_out(int chan, uint64_t now_ns) +{ + return tim1_cnt(now_ns) < tim1.ccr_act[chan]; +} + +/* + dead time from the DTG field of the emulated TIM1 BDTR, decoded with + the STM32 encoding at t_DTS = one 160MHz CPU tick (CKD is never + changed by the firmware). The firmware sets this the same way as on + hardware: DEAD_TIME at init plus dead_time_override ORed in from + loadEEpromSettings + */ +uint32_t sitl_tim1_dead_time_ns(void) +{ + static uint32_t last_bdtr = 0xffffffff; + static uint32_t dead_ns; + const uint32_t bdtr = tims[SITL_TIM1_IDX].BDTR; + if (bdtr != last_bdtr) { + last_bdtr = bdtr; + const uint32_t dtg = bdtr & 0xFF; + uint32_t ticks; + if ((dtg & 0x80) == 0) { + ticks = dtg; + } else if ((dtg & 0xC0) == 0x80) { + ticks = (64 + (dtg & 0x3F)) * 2; + } else if ((dtg & 0xE0) == 0xC0) { + ticks = (32 + (dtg & 0x1F)) * 8; + } else { + ticks = (32 + (dtg & 0x1F)) * 16; + } + dead_ns = (ticks * CPU_TICK_NS_NUM) / CPU_TICK_NS_DEN; + fprintf(stderr, "SITL: dead time %uns (BDTR 0x%02x)\n", dead_ns, dtg); + } + return dead_ns; +} + +void sitl_tim1_set_duty_all(uint16_t duty) +{ + tim1.ccr_pre[0] = duty; + tim1.ccr_pre[1] = duty; + tim1.ccr_pre[2] = duty; +} + +void sitl_tim1_set_duty(int chan, uint16_t duty) +{ + tim1.ccr_pre[chan] = duty; +} + +void sitl_tim1_set_psc(uint16_t psc) +{ + tim1.psc_pre = psc; +} + +void sitl_tim1_set_arr(uint16_t arr) +{ + tim1.arr_pre = arr; +} + +void sitl_tim1_force_update(void) +{ + tim1_latch(); + tim1.period_start_ns = sitl_time_ns(); +} + +/* + INTERVAL_TIMER (TIM2), 2MHz + */ +static uint32_t interval_ticks_since(uint64_t now_ns, uint64_t base_ns) +{ + return (uint32_t)((now_ns - base_ns) / 500U); +} + +uint32_t sitl_interval_timer_count(void) +{ + if (sitl_in_sim_thread()) { + // interrupt context: busy waits on the timer must see time advance + sitl_isr_read_tick(); + } else { + sitl_fw_read_tick(); + } + return tim2.cnt_base + interval_ticks_since(sitl_time_ns(), tim2.base_ns); +} + +void sitl_interval_timer_set(uint32_t cnt) +{ + tim2.base_ns = sitl_time_ns(); + tim2.cnt_base = cnt; +} + +/* + COM_TIMER (TIM16), 2MHz, update interrupt used to time commutation + */ +void sitl_com_int_arm(uint32_t time) +{ + extern void motor_log_event(int kind, uint32_t a, uint32_t b, uint32_t c); + motor_log_event(4 /*MEV_ZC_ACCEPT*/, time, 0, 0); + SITL_TIM_TypeDef* t = &tims[SITL_TIM16_IDX]; + tim16.base_ns = sitl_time_ns(); + tim16.cnt_base = 0; + t->ARR = time; + t->SR = 0; + // enable last: the sim thread only evaluates when DIER is set + t->DIER |= 1; +} + +void sitl_com_int_disable(void) +{ + tims[SITL_TIM16_IDX].DIER &= ~1U; +} + +void sitl_com_int_enable(void) +{ + tims[SITL_TIM16_IDX].DIER |= 1; +} + +void sitl_tenkhz_enable(void) +{ + tim6.next_due_ns = sitl_time_ns() + (tims[SITL_TIM6_IDX].ARR + 1) * 1000ULL; + tim6.enabled = true; +} + +/* + called by the sim thread after each physics step + */ +void sitl_timers_step(uint64_t now_ns) +{ + // TIM1 update events latch the preload registers + uint64_t period = tim1_period_ns(); + while (now_ns - tim1.period_start_ns >= period) { + tim1.period_start_ns += period; + tim1_latch(); + period = tim1_period_ns(); + } + + // 20kHz loop timer + if (tim6.enabled && now_ns >= tim6.next_due_ns) { + tim6.next_due_ns += (tims[SITL_TIM6_IDX].ARR + 1) * 1000ULL; + sitl_irq_pend(SITL_IRQ_TENKHZ); + } + + // commutation timer + SITL_TIM_TypeDef* t16 = &tims[SITL_TIM16_IDX]; + if (t16->DIER & 1) { + const uint32_t cnt = tim16.cnt_base + interval_ticks_since(now_ns, tim16.base_ns); + if (cnt >= t16->ARR) { + // counter wraps to zero and keeps running + tim16.base_ns = now_ns; + tim16.cnt_base = 0; + t16->SR |= 1; + sitl_irq_pend(SITL_IRQ_COM); + } + } +} + +/* + register style dereference, syncing CNT with simulated time + */ +SITL_TIM_TypeDef* sitl_tim_deref(int idx) +{ + SITL_TIM_TypeDef* t = &tims[idx]; + if (sitl_in_sim_thread()) { + // interrupt context: firmware busy waiting on a timer (delayMicros + // inside an IRQ handler) must still see time advance, as on real + // hardware where the timers keep counting during an interrupt + sitl_isr_read_tick(); + } else { + sitl_fw_read_tick(); + } + const uint64_t now = sitl_time_ns(); + switch (idx) { + case SITL_TIM1_IDX: + t->CNT = tim1_cnt(now); + break; + case SITL_TIM2_IDX: + t->CNT = tim2.cnt_base + interval_ticks_since(now, tim2.base_ns); + break; + case SITL_TIM16_IDX: + t->CNT = tim16.cnt_base + interval_ticks_since(now, tim16.base_ns); + break; + case SITL_TIM17_IDX: + t->CNT = (uint32_t)((now / 1000U) & 0xffff); + break; + default: + break; + } + return t; +} diff --git a/Mcu/SITL/dshot_test.py b/Mcu/SITL/dshot_test.py new file mode 100644 index 000000000..d098cd980 --- /dev/null +++ b/Mcu/SITL/dshot_test.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 +''' +headless PWM/DShot input test for the AM32 SITL + +streams frames to the SITL UDP input port, handles zero-throttle arming, +then applies throttle and reports BDShot replies and rates. + +note that the ESC must have INPUT_SIGNAL_TYPE set away from DRONECAN_IN +(5) or the input interrupts are disabled; use --input-type to set it over +DroneCAN first (1=DSHOT_IN, 2=SERVO_IN, 0=AUTO_IN) + +examples: + dshot_test.py --input-type 1 --type dshot300 --throttle 500 + dshot_test.py --type dshot600 --bidir --throttle 800 --edt + dshot_test.py --input-type 2 --type pwm --throttle 1500 +''' + +import argparse +import sys +import time + +import sitl_dshot as sd + + +def main(): + ap = argparse.ArgumentParser(description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument('--host', default='127.0.0.1') + ap.add_argument('--port', type=int, default=57733) + ap.add_argument('--type', default='dshot300', choices=sorted(sd.TYPE_NAMES.keys())) + ap.add_argument('--bidir', action='store_true', help='bidirectional DShot (idle high, inverted CRC)') + ap.add_argument('--edt', action='store_true', help='enable extended DShot telemetry after arming') + ap.add_argument('--rate', type=float, default=500, help='frame rate Hz') + ap.add_argument('--throttle', type=float, default=500, + help='DShot value 48..2047, or PWM pulse width in us') + ap.add_argument('--arm-time', type=float, default=1.6, help='zero throttle arming time') + ap.add_argument('--duration', type=float, default=10, help='run time at throttle') + ap.add_argument('--poles', type=int, default=14) + ap.add_argument('--bad-crc', type=float, default=0, help='fraction of frames sent with corrupted CRC') + ap.add_argument('--input-type', type=int, default=None, + help='first set INPUT_SIGNAL_TYPE over DroneCAN (0=auto 1=dshot 2=servo 5=dronecan), save and restart') + ap.add_argument('--can-uri', default='mcast:0') + args = ap.parse_args() + + if args.input_type is not None: + print('setting INPUT_SIGNAL_TYPE=%d over DroneCAN...' % args.input_type) + node = sd.set_dronecan_param('INPUT_SIGNAL_TYPE', args.input_type, uri=args.can_uri) + print('set on node %d, restarted' % node) + time.sleep(1.0) + + ptype = sd.TYPE_NAMES[args.type] + port = sd.InputPort(args.host, args.port) + period = 1.0 / args.rate + + zero = 1000 if ptype == sd.TYPE_PWM else 0 + + def send(value, telem=False, corrupt=False): + if ptype == sd.TYPE_PWM: + port.send_pwm(int(value)) + else: + port.send_dshot(int(value), ptype=ptype, telem=telem, + bidir=args.bidir, corrupt=corrupt) + + def stream(value, duration, label): + t0 = time.time() + next_send = t0 + last_report = t0 + nsent = 0 + erpm_period = None + edt = {} + badcrc = 0 + while time.time() - t0 < duration: + now = time.time() + # catch-up burst: coarse sleep granularity (VMs, CI runners) + # must not lower the average frame rate - the firmware's + # bidirectional auto-detect needs >100 frames before arming + # completes + burst = 0 + while now >= next_send and burst < 10: + next_send += period + corrupt = args.bad_crc > 0 and (nsent % max(1, int(1 / args.bad_crc))) == 0 + send(value, corrupt=corrupt) + nsent += 1 + burst += 1 + if now - next_send > 0.25: + next_send = now # fell too far behind, resync + for r in port.get_replies(): + kind, val = sd.decode_reply(r[3], edt_expected=args.edt) + if kind == 'erpm': + erpm_period = val + elif kind == 'badcrc': + badcrc += 1 + else: + edt[kind] = val + if now - last_report >= 1.0: + last_report = now + rpm = sd.erpm_period_to_rpm(erpm_period, args.poles) if erpm_period else 0 + extra = ' '.join('%s=%s' % kv for kv in sorted(edt.items())) + print('%s: sent=%u replies=%u rpm=%.0f badcrc=%u %s' + % (label, port.sent_count, port.reply_count, rpm, badcrc, extra)) + sys.stdout.flush() + time.sleep(0.0005) + + print('arming with zero throttle for %.1fs...' % args.arm_time) + stream(zero, args.arm_time, 'arm') + + if args.edt and ptype != sd.TYPE_PWM: + print('enabling extended DShot telemetry (cmd 13)...') + for _ in range(8): + send(sd.DSHOT_CMD_EDT_ENABLE, telem=True) + time.sleep(period) + + print('throttle %g for %.1fs...' % (args.throttle, args.duration)) + stream(args.throttle, args.duration, 'run') + + print('back to zero...') + stream(zero, 1.0, 'stop') + port.close() + + +if __name__ == '__main__': + main() diff --git a/Mcu/SITL/example.json b/Mcu/SITL/example.json new file mode 100644 index 000000000..b9045801d --- /dev/null +++ b/Mcu/SITL/example.json @@ -0,0 +1,30 @@ +{ + "motor": { + "kv": 900, + "poles": 14, + "resistance": 0.045, + "inductance": 2.1e-5, + "mutual_inductance": 0.0, + "inertia": 3.0e-5, + "damping": 1.0e-6, + "static_friction": 0.003, + "load_k_omega2": 1.0e-7 + }, + "battery": { + "voltage": 16.8, + "resistance": 0.012 + }, + "esc": { + "rds_on": 0.004, + "diode_vf": 0.7, + "temperature_c": 25 + }, + "sim": { + "physics_dt_ns": 500, + "loop_time_ns": 2000, + "isr_read_ns": 100, + "comparator_noise_mv": 5, + "comparator_hysteresis_mv": 15, + "watchdog_enabled": true + } +} diff --git a/Mcu/SITL/gui_ci_test.py b/Mcu/SITL/gui_ci_test.py new file mode 100644 index 000000000..6e59ef168 --- /dev/null +++ b/Mcu/SITL/gui_ci_test.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python3 +''' +CI test for the SITL GUI: runs the real GUI under Qt's offscreen +platform, drives it through the control port (arming, EDT, throttle, +scopes, motor view) and asserts on the telemetry it reports. + +usage: gui_ci_test.py --gui-python Mcu/SITL/venv/bin/python3 +''' + +from __future__ import annotations + +import argparse +import os +import re +import socket +import subprocess +import sys +import threading +import time + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) +from sitl_harness import Sitl, find_sitl_binary, free_udp_port # noqa: E402 + +HERE = os.path.dirname(os.path.abspath(__file__)) +failures = [] + + +def check(name, cond, detail): + status = 'PASS' if cond else 'FAIL' + print('%s: %s (%s)' % (status, name, detail)) + sys.stdout.flush() + if not cond: + failures.append(name) + + +def main(): + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument('--gui-python', required=True) + ap.add_argument('--sitl', default=None) + args = ap.parse_args() + + sitl_path = find_sitl_binary(args.sitl) + if not sitl_path or not os.path.exists(sitl_path): + print('SITL binary not found: %s' % sitl_path) + sys.exit(2) + + env = dict(os.environ) + env['QT_QPA_PLATFORM'] = 'offscreen' + control_port = free_udp_port() + # free_udp_port returns a UDP port; TCP control port needs its own bind. + # Re-bind via TCP to get a free TCP port. + sprobe = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sprobe.bind(('127.0.0.1', 0)) + control_port = sprobe.getsockname()[1] + sprobe.close() + + # BDShot/EDT path only — keep CAN off so the GUI does not block on + # mcast/SocketCAN bring-up (DroneCAN is covered by other tests). + with Sitl(sitl_path, ['--input-type', '1'], can_uri='none') as sitl: + gui = subprocess.Popen( + [args.gui_python, os.path.join(HERE, 'sitl_gui.py'), + '--control-port', str(control_port), + '--port', str(sitl.input_port), + '--state-port', str(sitl.state_port), + '--can-uri', 'none'], + stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, env=env) + + # first launch can be slow (Qt font cache); retry the connection + s = None + deadline = time.time() + 45 + while time.time() < deadline: + try: + s = socket.create_connection(('127.0.0.1', control_port), timeout=5) + break + except OSError: + if gui.poll() is not None: + print(gui.stdout.read() if gui.stdout else '') + print('gui exited before control port came up') + sys.exit(1) + time.sleep(1.0) + if s is None: + gui.kill() + print('control port never came up') + sys.exit(1) + # connect() timeout must not apply to the long idle gaps between + # scripted commands (arm + EDT hold is several seconds). + s.settimeout(None) + f = s.makefile('r') + responses = [] + + def reader(): + try: + for line in f: + responses.append(line.rstrip()) + except OSError: + pass + + threading.Thread(target=reader, daemon=True).start() + + # Hold zero throttle long enough for bidir auto-detect, arming, and + # EDT enable (firmware needs 6 identical DShot cmds while stopped). + for delay, cmd in [ + (0.1, 'ds_type dshot600'), (0.1, 'ds_bidir 1'), + (0.1, 'ds_enable 1'), (0.5, 'ds_edt 1'), + (4.0, 'ds_value 900'), + (1.0, 'graph_i 1'), (0.2, 'graph_v 1'), (0.2, 'motorview 1'), + (4.0, 'status'), + (1.0, 'quit')]: + time.sleep(delay) + s.sendall((cmd + '\n').encode()) + + try: + gui.wait(timeout=20) + check('gui exits cleanly', gui.returncode == 0, + 'exit=%s' % gui.returncode) + except subprocess.TimeoutExpired: + gui.kill() + check('gui exits cleanly', False, 'hung') + + bds = [r for r in responses if r.startswith('STATUS BDShot')] + check('gui got BDShot status', len(bds) > 0, '%d lines' % len(bds)) + if bds: + m = re.search(r'rpm=(\d+)\s+(\w+)\s+EDT:(\w+)', bds[-1]) + check('gui status parses', m is not None, bds[-1]) + if m: + rpm, spin, edt = int(m.group(1)), m.group(2), m.group(3) + check('gui rpm', 4000 <= rpm <= 7000, 'rpm=%d' % rpm) + check('gui spinning', spin == 'spinning', spin) + check('gui edt on', edt == 'on', 'EDT:%s' % edt) + out = gui.stdout.read() if gui.stdout else '' + check('gui no tracebacks', 'Traceback' not in out, + (out[-300:] if 'Traceback' in out else 'clean')) + + if failures: + print('\n%d FAILED' % len(failures)) + sys.exit(1) + print('\ngui test passed') + + +if __name__ == '__main__': + main() diff --git a/Mcu/SITL/make_gui_env.py b/Mcu/SITL/make_gui_env.py new file mode 100644 index 000000000..a44fb94cd --- /dev/null +++ b/Mcu/SITL/make_gui_env.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +''' +create a self-contained python environment for the SITL GUI in +Mcu/SITL/venv and install the GUI dependencies (PySide6, pyqtgraph, +dronecan) into it. Works the same on Linux, Windows and macOS. + +usage: python3 make_gui_env.py +then run the GUI with the interpreter this prints. +''' + +import os +import subprocess +import sys +import venv + +here = os.path.dirname(os.path.abspath(__file__)) +env_dir = os.path.join(here, 'venv') +requirements = os.path.join(here, 'requirements-gui.txt') + +if sys.platform == 'win32': + python = os.path.join(env_dir, 'Scripts', 'python.exe') +else: + python = os.path.join(env_dir, 'bin', 'python3') + + +def has_pip(py): + try: + subprocess.check_call([py, '-m', 'pip', '--version'], + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL) + return True + except (subprocess.CalledProcessError, OSError): + return False + + +def bootstrap_pip(py): + '''Ubuntu/Debian venvs sometimes lack pip even with with_pip=True + (missing ensurepip package, or a half-created venv).''' + print('bootstrapping pip into the venv ...') + try: + subprocess.check_call([py, '-m', 'ensurepip', '--upgrade']) + except subprocess.CalledProcessError: + # last resort: get-pip.py style via pip from the host + if has_pip(sys.executable): + subprocess.check_call( + [sys.executable, '-m', 'pip', 'install', '--upgrade', + 'pip', '--target', + os.path.join(env_dir, + 'Lib' if sys.platform == 'win32' else + 'lib/python%d.%d/site-packages' % sys.version_info[:2])]) + else: + raise SystemExit( + 'venv has no pip and ensurepip failed.\n' + 'On Ubuntu/Debian install: sudo apt install python3-venv python3-pip\n' + 'Then: rm -rf Mcu/SITL/venv && python3 Mcu/SITL/make_gui_env.py') + + +if not os.path.exists(python): + print('creating %s ...' % env_dir) + # with_pip can still fail silently on some distros; we check below + try: + venv.EnvBuilder(with_pip=True).create(env_dir) + except Exception as e: + print('venv create with_pip failed (%s), retrying without ...' % e) + venv.EnvBuilder(with_pip=False).create(env_dir) + +if not os.path.exists(python): + raise SystemExit('venv python not found at %s' % python) + +if not has_pip(python): + bootstrap_pip(python) +if not has_pip(python): + raise SystemExit( + 'still no pip in %s after bootstrap.\n' + 'Try: sudo apt install python3-venv python3-pip\n' + ' rm -rf Mcu/SITL/venv && python3 Mcu/SITL/make_gui_env.py' % env_dir) + +print('installing GUI dependencies ...') +subprocess.check_call([python, '-m', 'pip', 'install', '--upgrade', + '-r', requirements]) + +print('\nGUI environment ready. Run the GUI with:') +print(' %s %s' % (python, os.path.join(here, 'sitl_gui.py'))) diff --git a/Mcu/SITL/models/default_7inch.json b/Mcu/SITL/models/default_7inch.json new file mode 100644 index 000000000..c52a118bb --- /dev/null +++ b/Mcu/SITL/models/default_7inch.json @@ -0,0 +1,13 @@ +{ + "motor": { + "kv": 900, + "poles": 14, + "resistance": 0.045, + "inductance": 2.1e-5, + "inertia": 3.0e-5, + "damping": 1.0e-6, + "static_friction": 0.003, + "load_k_omega2": 1.0e-7 + }, + "battery": { "voltage": 16.8, "resistance": 0.012 } +} diff --git a/Mcu/SITL/models/heavy_13inch.json b/Mcu/SITL/models/heavy_13inch.json new file mode 100644 index 000000000..0e2f7b48b --- /dev/null +++ b/Mcu/SITL/models/heavy_13inch.json @@ -0,0 +1,13 @@ +{ + "motor": { + "kv": 360, + "poles": 22, + "resistance": 0.09, + "inductance": 4.5e-5, + "inertia": 2.5e-4, + "damping": 3.0e-6, + "static_friction": 0.006, + "load_k_omega2": 1.0e-6 + }, + "battery": { "voltage": 25.2, "resistance": 0.015 } +} diff --git a/Mcu/SITL/models/racer_5inch.json b/Mcu/SITL/models/racer_5inch.json new file mode 100644 index 000000000..87276c165 --- /dev/null +++ b/Mcu/SITL/models/racer_5inch.json @@ -0,0 +1,13 @@ +{ + "motor": { + "kv": 1750, + "poles": 14, + "resistance": 0.025, + "inductance": 8.0e-6, + "inertia": 8.0e-6, + "damping": 5.0e-7, + "static_friction": 0.002, + "load_k_omega2": 2.0e-8 + }, + "battery": { "voltage": 25.2, "resistance": 0.010 } +} diff --git a/Mcu/SITL/models/unloaded.json b/Mcu/SITL/models/unloaded.json new file mode 100644 index 000000000..a591cba5a --- /dev/null +++ b/Mcu/SITL/models/unloaded.json @@ -0,0 +1,13 @@ +{ + "motor": { + "kv": 900, + "poles": 14, + "resistance": 0.045, + "inductance": 2.1e-5, + "inertia": 2.0e-5, + "damping": 1.0e-6, + "static_friction": 0.003, + "load_k_omega2": 2.0e-9 + }, + "battery": { "voltage": 16.8, "resistance": 0.012 } +} diff --git a/Mcu/SITL/requirements-ci.txt b/Mcu/SITL/requirements-ci.txt new file mode 100644 index 000000000..874498475 --- /dev/null +++ b/Mcu/SITL/requirements-ci.txt @@ -0,0 +1,3 @@ +# Headless SITL CI dependencies (no GUI) +dronecan +pytest>=7.0 diff --git a/Mcu/SITL/requirements-gui.txt b/Mcu/SITL/requirements-gui.txt new file mode 100644 index 000000000..95b0ed931 --- /dev/null +++ b/Mcu/SITL/requirements-gui.txt @@ -0,0 +1,4 @@ +# python dependencies for the SITL control GUI (sitl_gui.py) +PySide6 +pyqtgraph +dronecan diff --git a/Mcu/SITL/run_ci_tests.py b/Mcu/SITL/run_ci_tests.py new file mode 100644 index 000000000..9bed081e9 --- /dev/null +++ b/Mcu/SITL/run_ci_tests.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 +''' +CI entry point for the AM32 SITL test suite. + +Prefers pytest (Mcu/SITL/tests/). Falls back to a small inline suite when +pytest is not installed, so the script stays usable with only the stdlib +plus optional pydronecan. + +usage: + python3 Mcu/SITL/run_ci_tests.py [--sitl path/to/elf] [-- pytest-args...] +exits non-zero if any test fails. +''' + +from __future__ import annotations + +import argparse +import os +import sys +import time + +HERE = os.path.dirname(os.path.abspath(__file__)) +sys.path.insert(0, HERE) + +from sitl_harness import ( # noqa: E402 + Sender, + Sitl, + find_sitl_binary, + free_mcast_group, + open_state, + rpm_from_state, + wait_for_state, +) +import sitl_dshot as sd # noqa: E402 + +failures = [] + + +def check(name, cond, detail): + status = 'PASS' if cond else 'FAIL' + print('%s: %s (%s)' % (status, name, detail)) + sys.stdout.flush() + if not cond: + failures.append(name) + + +def test_dshot(sitl_path, name, ptype, bidir, edt, value, rpm_lo, rpm_hi, input_type=1): + with Sitl(sitl_path, ['--input-type', str(input_type)], can_uri='none') as sitl: + sim = open_state('127.0.0.1', sitl.state_port, period_us=200) + tx = Sender('127.0.0.1', sitl.input_port, ptype, bidir=bidir) + try: + time.sleep(2.2) + if edt: + tx.cmds = [sd.DSHOT_CMD_EDT_ENABLE] * 8 + time.sleep(0.5) + tx.value = value + time.sleep(4.0) + rpm = rpm_from_state(sim) + check(name + ' rpm', rpm_lo <= rpm <= rpm_hi, + 'rpm=%.0f expected %d..%d' % (rpm, rpm_lo, rpm_hi)) + if bidir: + replies = tx.port.reply_count + check(name + ' bdshot replies', replies > 500, + 'replies=%d' % replies) + erpm = [sd.decode_reply(r[3], edt_expected=edt) + for r in tx.port.get_replies()] + rpms = [sd.erpm_period_to_rpm(v) for k, v in erpm if k == 'erpm'] + if rpms: + check(name + ' bdshot rpm agrees', + abs(rpms[-1] - rpm) < max(200, rpm * 0.05), + 'bdshot=%.0f state=%.0f' % (rpms[-1], rpm)) + if edt: + edt_vals = dict((k, v) for k, v in erpm + if k in ('temp', 'volt', 'current')) + check(name + ' edt values', + edt_vals.get('temp') == 25 and 14 < edt_vals.get('volt', 0) < 18, + 'edt=%s' % edt_vals) + tx.value = 1000 if ptype == sd.TYPE_PWM else 0 + time.sleep(3.0) + rpm = rpm_from_state(sim, 0.3) + check(name + ' stops', rpm < 500, 'rpm=%.0f' % rpm) + finally: + tx.stop() + sim.close() + + +def test_dronecan(sitl_path): + try: + import dronecan + except ImportError: + print('SKIP: dronecan not installed, DroneCAN test skipped') + return + can_uri = 'mcast:%d' % free_mcast_group() + with Sitl(sitl_path, ['--node-id', '10'], can_uri=can_uri, wait_s=1.0) as sitl: + sim = open_state('127.0.0.1', sitl.state_port, period_us=200) + if not wait_for_state(sim, timeout=5.0): + print('SKIP: SITL state stream never started with CAN enabled, ' + 'multicast is probably unavailable on this host. SITL log tail:') + sys.stdout.flush() + print(sitl.log_tail(5)) + sim.close() + return + node = dronecan.make_node(can_uri, node_id=100, bitrate=1000000) + status = {} + + def on_esc(e): + status['rpm'] = e.message.rpm + status['voltage'] = e.message.voltage + + node.add_handler(dronecan.uavcan.equipment.esc.Status, on_esc) + t0 = time.time() + nxt = t0 + while time.time() - t0 < 10: + node.spin(0) + now = time.time() + if now >= nxt: + nxt += 0.02 + thr = 0.35 if now - t0 > 2.5 else 0.0 + node.broadcast(dronecan.uavcan.equipment.safety.ArmingStatus(status=255)) + node.broadcast(dronecan.uavcan.equipment.esc.RawCommand(cmd=[int(8191 * thr)])) + time.sleep(0.001) + rpm = rpm_from_state(sim) + check('dronecan rpm', 3500 <= rpm <= 6500, 'rpm=%.0f' % rpm) + check('dronecan telemetry', 3500 <= status.get('rpm', -1) <= 6500 + and 15 < status.get('voltage', 0) < 18, + 'esc.Status=%s' % status) + node.close() + sim.close() + + +def run_legacy(sitl_path): + test_dshot(sitl_path, 'dshot600 bidir edt', sd.TYPE_DSHOT600, + bidir=True, edt=True, value=800, rpm_lo=4000, rpm_hi=7000) + test_dshot(sitl_path, 'dshot300', sd.TYPE_DSHOT300, + bidir=False, edt=False, value=600, rpm_lo=3000, rpm_hi=6000) + test_dshot(sitl_path, 'pwm', sd.TYPE_PWM, + bidir=False, edt=False, value=1500, rpm_lo=4000, rpm_hi=9000, + input_type=2) + test_dronecan(sitl_path) + if failures: + print('\n%d FAILED: %s' % (len(failures), ', '.join(failures))) + return 1 + print('\nall tests passed') + return 0 + + +def run_pytest(sitl_path, extra): + import pytest + args = [ + os.path.join(HERE, 'tests'), + '-v', + '--tb=short', + '--sitl', sitl_path, + ] + args += extra + return pytest.main(args) + + +def main(): + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument('--sitl', default=None, help='path to SITL binary') + ap.add_argument('--legacy', action='store_true', + help='run the small stdlib suite instead of pytest') + args, rest = ap.parse_known_args() + + sitl_path = find_sitl_binary(args.sitl) + if not sitl_path or not os.path.exists(sitl_path): + print('SITL binary not found: %s' % sitl_path) + sys.exit(2) + + if not args.legacy: + try: + import pytest # noqa: F401 + except ImportError: + print('pytest not installed; falling back to legacy suite ' + '(pip install -r Mcu/SITL/requirements-ci.txt)') + args.legacy = True + + if args.legacy: + sys.exit(run_legacy(sitl_path)) + sys.exit(run_pytest(sitl_path, rest)) + + +if __name__ == '__main__': + main() diff --git a/Mcu/SITL/sim/jsmn.c b/Mcu/SITL/sim/jsmn.c new file mode 100644 index 000000000..e7765eb1d --- /dev/null +++ b/Mcu/SITL/sim/jsmn.c @@ -0,0 +1,311 @@ +#include "jsmn.h" + +/** + * Allocates a fresh unused token from the token pull. + */ +static jsmntok_t *jsmn_alloc_token(jsmn_parser *parser, + jsmntok_t *tokens, size_t num_tokens) { + jsmntok_t *tok; + if (parser->toknext >= num_tokens) { + return NULL; + } + tok = &tokens[parser->toknext++]; + tok->start = tok->end = -1; + tok->size = 0; +#ifdef JSMN_PARENT_LINKS + tok->parent = -1; +#endif + return tok; +} + +/** + * Fills token type and boundaries. + */ +static void jsmn_fill_token(jsmntok_t *token, jsmntype_t type, + int start, int end) { + token->type = type; + token->start = start; + token->end = end; + token->size = 0; +} + +/** + * Fills next available token with JSON primitive. + */ +static int jsmn_parse_primitive(jsmn_parser *parser, const char *js, + size_t len, jsmntok_t *tokens, size_t num_tokens) { + jsmntok_t *token; + int start; + + start = parser->pos; + + for (; parser->pos < len && js[parser->pos] != '\0'; parser->pos++) { + switch (js[parser->pos]) { +#ifndef JSMN_STRICT + /* In strict mode primitive must be followed by "," or "}" or "]" */ + case ':': +#endif + case '\t' : case '\r' : case '\n' : case ' ' : + case ',' : case ']' : case '}' : + goto found; + } + if (js[parser->pos] < 32 || js[parser->pos] >= 127) { + parser->pos = start; + return JSMN_ERROR_INVAL; + } + } +#ifdef JSMN_STRICT + /* In strict mode primitive must be followed by a comma/object/array */ + parser->pos = start; + return JSMN_ERROR_PART; +#endif + +found: + if (tokens == NULL) { + parser->pos--; + return 0; + } + token = jsmn_alloc_token(parser, tokens, num_tokens); + if (token == NULL) { + parser->pos = start; + return JSMN_ERROR_NOMEM; + } + jsmn_fill_token(token, JSMN_PRIMITIVE, start, parser->pos); +#ifdef JSMN_PARENT_LINKS + token->parent = parser->toksuper; +#endif + parser->pos--; + return 0; +} + +/** + * Fills next token with JSON string. + */ +static int jsmn_parse_string(jsmn_parser *parser, const char *js, + size_t len, jsmntok_t *tokens, size_t num_tokens) { + jsmntok_t *token; + + int start = parser->pos; + + parser->pos++; + + /* Skip starting quote */ + for (; parser->pos < len && js[parser->pos] != '\0'; parser->pos++) { + char c = js[parser->pos]; + + /* Quote: end of string */ + if (c == '\"') { + if (tokens == NULL) { + return 0; + } + token = jsmn_alloc_token(parser, tokens, num_tokens); + if (token == NULL) { + parser->pos = start; + return JSMN_ERROR_NOMEM; + } + jsmn_fill_token(token, JSMN_STRING, start+1, parser->pos); +#ifdef JSMN_PARENT_LINKS + token->parent = parser->toksuper; +#endif + return 0; + } + + /* Backslash: Quoted symbol expected */ + if (c == '\\' && parser->pos + 1 < len) { + int i; + parser->pos++; + switch (js[parser->pos]) { + /* Allowed escaped symbols */ + case '\"': case '/' : case '\\' : case 'b' : + case 'f' : case 'r' : case 'n' : case 't' : + break; + /* Allows escaped symbol \uXXXX */ + case 'u': + parser->pos++; + for(i = 0; i < 4 && parser->pos < len && js[parser->pos] != '\0'; i++) { + /* If it isn't a hex character we have an error */ + if(!((js[parser->pos] >= 48 && js[parser->pos] <= 57) || /* 0-9 */ + (js[parser->pos] >= 65 && js[parser->pos] <= 70) || /* A-F */ + (js[parser->pos] >= 97 && js[parser->pos] <= 102))) { /* a-f */ + parser->pos = start; + return JSMN_ERROR_INVAL; + } + parser->pos++; + } + parser->pos--; + break; + /* Unexpected symbol */ + default: + parser->pos = start; + return JSMN_ERROR_INVAL; + } + } + } + parser->pos = start; + return JSMN_ERROR_PART; +} + +/** + * Parse JSON string and fill tokens. + */ +int jsmn_parse(jsmn_parser *parser, const char *js, size_t len, + jsmntok_t *tokens, unsigned int num_tokens) { + int r; + int i; + jsmntok_t *token; + int count = parser->toknext; + + for (; parser->pos < len && js[parser->pos] != '\0'; parser->pos++) { + char c; + jsmntype_t type; + + c = js[parser->pos]; + switch (c) { + case '{': case '[': + count++; + if (tokens == NULL) { + break; + } + token = jsmn_alloc_token(parser, tokens, num_tokens); + if (token == NULL) + return JSMN_ERROR_NOMEM; + if (parser->toksuper != -1) { + tokens[parser->toksuper].size++; +#ifdef JSMN_PARENT_LINKS + token->parent = parser->toksuper; +#endif + } + token->type = (c == '{' ? JSMN_OBJECT : JSMN_ARRAY); + token->start = parser->pos; + parser->toksuper = parser->toknext - 1; + break; + case '}': case ']': + if (tokens == NULL) + break; + type = (c == '}' ? JSMN_OBJECT : JSMN_ARRAY); +#ifdef JSMN_PARENT_LINKS + if (parser->toknext < 1) { + return JSMN_ERROR_INVAL; + } + token = &tokens[parser->toknext - 1]; + for (;;) { + if (token->start != -1 && token->end == -1) { + if (token->type != type) { + return JSMN_ERROR_INVAL; + } + token->end = parser->pos + 1; + parser->toksuper = token->parent; + break; + } + if (token->parent == -1) { + break; + } + token = &tokens[token->parent]; + } +#else + for (i = parser->toknext - 1; i >= 0; i--) { + token = &tokens[i]; + if (token->start != -1 && token->end == -1) { + if (token->type != type) { + return JSMN_ERROR_INVAL; + } + parser->toksuper = -1; + token->end = parser->pos + 1; + break; + } + } + /* Error if unmatched closing bracket */ + if (i == -1) return JSMN_ERROR_INVAL; + for (; i >= 0; i--) { + token = &tokens[i]; + if (token->start != -1 && token->end == -1) { + parser->toksuper = i; + break; + } + } +#endif + break; + case '\"': + r = jsmn_parse_string(parser, js, len, tokens, num_tokens); + if (r < 0) return r; + count++; + if (parser->toksuper != -1 && tokens != NULL) + tokens[parser->toksuper].size++; + break; + case '\t' : case '\r' : case '\n' : case ' ': + break; + case ':': + parser->toksuper = parser->toknext - 1; + break; + case ',': + if (tokens != NULL && parser->toksuper != -1 && + tokens[parser->toksuper].type != JSMN_ARRAY && + tokens[parser->toksuper].type != JSMN_OBJECT) { +#ifdef JSMN_PARENT_LINKS + parser->toksuper = tokens[parser->toksuper].parent; +#else + for (i = parser->toknext - 1; i >= 0; i--) { + if (tokens[i].type == JSMN_ARRAY || tokens[i].type == JSMN_OBJECT) { + if (tokens[i].start != -1 && tokens[i].end == -1) { + parser->toksuper = i; + break; + } + } + } +#endif + } + break; +#ifdef JSMN_STRICT + /* In strict mode primitives are: numbers and booleans */ + case '-': case '0': case '1' : case '2': case '3' : case '4': + case '5': case '6': case '7' : case '8': case '9': + case 't': case 'f': case 'n' : + /* And they must not be keys of the object */ + if (tokens != NULL && parser->toksuper != -1) { + jsmntok_t *t = &tokens[parser->toksuper]; + if (t->type == JSMN_OBJECT || + (t->type == JSMN_STRING && t->size != 0)) { + return JSMN_ERROR_INVAL; + } + } +#else + /* In non-strict mode every unquoted value is a primitive */ + default: +#endif + r = jsmn_parse_primitive(parser, js, len, tokens, num_tokens); + if (r < 0) return r; + count++; + if (parser->toksuper != -1 && tokens != NULL) + tokens[parser->toksuper].size++; + break; + +#ifdef JSMN_STRICT + /* Unexpected char in strict mode */ + default: + return JSMN_ERROR_INVAL; +#endif + } + } + + if (tokens != NULL) { + for (i = parser->toknext - 1; i >= 0; i--) { + /* Unmatched opened object or array */ + if (tokens[i].start != -1 && tokens[i].end == -1) { + return JSMN_ERROR_PART; + } + } + } + + return count; +} + +/** + * Creates a new parser based over a given buffer with an array of tokens + * available. + */ +void jsmn_init(jsmn_parser *parser) { + parser->pos = 0; + parser->toknext = 0; + parser->toksuper = -1; +} + diff --git a/Mcu/SITL/sim/jsmn.h b/Mcu/SITL/sim/jsmn.h new file mode 100644 index 000000000..01ca99c8e --- /dev/null +++ b/Mcu/SITL/sim/jsmn.h @@ -0,0 +1,76 @@ +#ifndef __JSMN_H_ +#define __JSMN_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * JSON type identifier. Basic types are: + * o Object + * o Array + * o String + * o Other primitive: number, boolean (true/false) or null + */ +typedef enum { + JSMN_UNDEFINED = 0, + JSMN_OBJECT = 1, + JSMN_ARRAY = 2, + JSMN_STRING = 3, + JSMN_PRIMITIVE = 4 +} jsmntype_t; + +enum jsmnerr { + /* Not enough tokens were provided */ + JSMN_ERROR_NOMEM = -1, + /* Invalid character inside JSON string */ + JSMN_ERROR_INVAL = -2, + /* The string is not a full JSON packet, more bytes expected */ + JSMN_ERROR_PART = -3 +}; + +/** + * JSON token description. + * @param type type (object, array, string etc.) + * @param start start position in JSON data string + * @param end end position in JSON data string + */ +typedef struct { + jsmntype_t type; + int start; + int end; + int size; +#ifdef JSMN_PARENT_LINKS + int parent; +#endif +} jsmntok_t; + +/** + * JSON parser. Contains an array of token blocks available. Also stores + * the string being parsed now and current position in that string + */ +typedef struct { + unsigned int pos; /* offset in the JSON string */ + unsigned int toknext; /* next token to allocate */ + int toksuper; /* superior token node, e.g parent object or array */ +} jsmn_parser; + +/** + * Create JSON parser over an array of tokens + */ +void jsmn_init(jsmn_parser *parser); + +/** + * Run JSON parser. It parses a JSON data string into and array of tokens, each describing + * a single JSON object. + */ +int jsmn_parse(jsmn_parser *parser, const char *js, size_t len, + jsmntok_t *tokens, unsigned int num_tokens); + +#ifdef __cplusplus +} +#endif + +#endif /* __JSMN_H_ */ diff --git a/Mcu/SITL/sim/motor.c b/Mcu/SITL/sim/motor.c new file mode 100644 index 000000000..21042a651 --- /dev/null +++ b/Mcu/SITL/sim/motor.c @@ -0,0 +1,604 @@ +/* + motor.c - trapezoidal BEMF BLDC motor, 3 phase bridge and battery model + for AM32 SITL. + + The electrical model follows the approach of open-bldc-csim + (https://github.com/open-bldc/open-bldc-csim, GPLv3+, Piotr + Esden-Tempski), which in turn implements Kang & Yoo, "Switching + Pattern-Independent Simulation Model for Brushless DC Motors", Journal + of Power Electronics 11-2, 2011: + https://jpels.org/digital-library/manuscript/file/17706/8_JPE-10238.pdf + + As in the paper, each step finds the conducting phases from the switch + and diode states, computes the motor neutral as the average of (v - e) + over the conducting phases (paper eq 10) and integrates + v = R*i + (L-M)*di/dt + e + v_m for each of them (paper eq 1), so the + floating phase terminal voltage (and therefore the BEMF zero crossing + seen by the comparator) is physical for any switching pattern. + + Differences from the paper: + - gate states come from the emulated TIM1 compare outputs and the AM32 + phase modes rather than switching functions, which adds dead time + windows with body diode conduction + - fets have Rds_on, and the diode Vf appears in the clamped terminal + voltage, not only in the diode on/off conditions + - the battery has internal resistance, so Vdc sags with load instead + of being stiff + - the load model adds a k*omega^2 propeller torque and static friction + to the paper's viscous damping + - torque uses the flux linkage form kt*sum(shape*i) (the second form + of paper eq 2), which is valid at omega = 0 + - integration is forward euler at 500ns steps rather than ode45 at + 2.5us + + Conventions: + - phase currents are positive INTO the motor terminal + - terminal voltages are referenced to battery negative + - theta is the mechanical rotor angle in radians + */ + +#include "motor.h" + +#include +#include +#include +#include + +#include "sitl.h" +#include "sitl_config.h" +#include "eeprom.h" + +#define TWO_PI 6.283185307179586 + +// which EXTI line the active comparator drives, owned by comparator.c +extern uint32_t current_EXTI_LINE; + +static void check_desync_dump(void); +static void dump_ring(void); + +static struct { + double theta; // mechanical angle, rad + double omega; // mechanical speed, rad/s + double i[3]; // phase currents, A + double ke; // V/(rad/s) mechanical + double vbus; // battery terminal voltage after sag + double ibus; // battery current, previous step (breaks the loop) + // dead time tracking: per phase last PWM level and the end of the + // current both-off window + bool pwm_last[3]; + uint64_t dead_until[3]; + // last terminal voltages for the state stream + double v_term[3]; + // sensor averaging accumulators + double acc_v, acc_i; + uint32_t acc_n; + unsigned rand_seed; + // seqlock for the sensor snapshot + volatile uint32_t seq; + sitl_sensors_t sensors; +} m; + +void motor_config_changed(void) +{ + // per phase BEMF constant [V s/rad] from Kv [rpm/V]. Kv is defined + // line to line and two phases conduct in series, so halve it. All + // other motor parameters are read from sitl_cfg on every step + m.ke = 0.5 * 60.0 / (TWO_PI * sitl_cfg.motor.kv); +} + +void motor_add_signals(double acc[8]) +{ + for (int p = 0; p < 3; p++) { + acc[p] += m.i[p]; + acc[3 + p] += m.v_term[p]; + } + acc[6] += m.vbus; + acc[7] += m.ibus; +} + +void motor_get_live_state(float* omega, float* theta, float* theta_e, + float i[3], float v[3], float* vbus, float* ibus) +{ + for (int p = 0; p < 3; p++) { + v[p] = (float)m.v_term[p]; + } + const int pole_pairs = sitl_cfg.motor.poles / 2; + *omega = (float)m.omega; + double th = fmod(m.theta, TWO_PI); + if (th < 0) { + th += TWO_PI; + } + *theta = (float)th; + double the = fmod(m.theta * pole_pairs, TWO_PI); + if (the < 0) { + the += TWO_PI; + } + *theta_e = (float)the; + for (int p = 0; p < 3; p++) { + i[p] = (float)m.i[p]; + } + *vbus = (float)m.vbus; + *ibus = (float)m.ibus; +} + +void motor_init(void) +{ + memset(&m, 0, sizeof(m)); + motor_config_changed(); + m.vbus = sitl_cfg.battery.voltage; + m.rand_seed = 12345; + m.sensors.bus_voltage = m.vbus; + m.sensors.temperature_c = sitl_cfg.esc.temperature_c; +} + +/* + normalised trapezoidal BEMF shape over one electrical revolution. + Rising zero crossing at 0, falling at pi, flat top from pi/6..5pi/6 + */ +static double trap_shape(double thetae) +{ + thetae = fmod(thetae, TWO_PI); + if (thetae < 0) { + thetae += TWO_PI; + } + const double s = M_PI / 6; // 30 degree ramp + if (thetae < s) { + return thetae / s; + } + if (thetae < M_PI - s) { + return 1.0; + } + if (thetae < M_PI + s) { + return (M_PI - thetae) / s; + } + if (thetae < TWO_PI - s) { + return -1.0; + } + return (thetae - TWO_PI) / s; +} + +void sitl_sensors_write(const sitl_sensors_t* in) +{ + m.seq++; + __atomic_thread_fence(__ATOMIC_SEQ_CST); + m.sensors = *in; + __atomic_thread_fence(__ATOMIC_SEQ_CST); + m.seq++; +} + +void sitl_sensors_read(sitl_sensors_t* out) +{ + for (;;) { + const uint32_t s1 = m.seq; + __atomic_thread_fence(__ATOMIC_SEQ_CST); + *out = m.sensors; + __atomic_thread_fence(__ATOMIC_SEQ_CST); + if (s1 == m.seq && (s1 & 1) == 0) { + return; + } + } +} + +void motor_step(uint64_t now_ns, uint32_t dt_ns) +{ + const double dt = dt_ns * 1e-9; + const int pole_pairs = sitl_cfg.motor.poles / 2; + const double R = sitl_cfg.motor.resistance; + const double L_eff = sitl_cfg.motor.inductance - sitl_cfg.motor.mutual_inductance; + const double rds = sitl_cfg.esc.rds_on; + const double vf = sitl_cfg.esc.diode_vf; + const double i_eps = 0.01; // A, diode turn off threshold + + // battery sag from last step's bus current + double vbus = sitl_cfg.battery.voltage - m.ibus * sitl_cfg.battery.resistance; + if (vbus < 0) { + vbus = 0; + } + m.vbus = vbus; + + // BEMF per phase + // phase order such that the AM32 comStep sequence 1..6 advances the + // field by +60 degrees electrical per step (verified by detent test) + const double thetae = m.theta * pole_pairs; + double e[3], shape[3]; + for (int p = 0; p < 3; p++) { + shape[p] = trap_shape(thetae + p * (TWO_PI / 3.0)); + e[p] = m.ke * m.omega * shape[p]; + } + + // gate states from the phase mode and the emulated PWM timer. On a + // complementary switched phase both fets are off for the configured + // dead time after each PWM edge and the body diode conducts, as on + // hardware + const uint32_t dead_ns = sitl_tim1_dead_time_ns(); + bool hi[3], lo[3]; + for (int p = 0; p < 3; p++) { + const bool pwm = sitl_tim1_pwm_out(p, now_ns); + switch (sitl_phase_mode[p]) { + case SITL_PHASE_PWM: + if (pwm != m.pwm_last[p]) { + m.pwm_last[p] = pwm; + m.dead_until[p] = now_ns + dead_ns; + } + if (now_ns < m.dead_until[p]) { + hi[p] = false; + lo[p] = false; + break; + } + hi[p] = pwm; + lo[p] = !pwm; + break; + case SITL_PHASE_PWM_NOCOMP: + hi[p] = pwm; + lo[p] = false; + break; + case SITL_PHASE_LOW: + hi[p] = false; + lo[p] = true; + break; + case SITL_PHASE_BRAKE_PWM: + hi[p] = false; + lo[p] = !pwm; + break; + case SITL_PHASE_FLOAT: + default: + hi[p] = false; + lo[p] = false; + break; + } + if (sitl_phase_mode[p] != SITL_PHASE_PWM) { + m.pwm_last[p] = pwm; + m.dead_until[p] = 0; + } + } + + // terminal voltages (paper eq 9); conducting[] is the excited set of + // paper eq 8. A driven phase is tied to the rail through the fet; an + // undriven phase carrying current is clamped by a body diode; an + // undriven phase without current floats at e + v_star + double v[3]; + double r_eff[3]; + bool conducting[3]; + for (int p = 0; p < 3; p++) { + r_eff[p] = R; + if (hi[p]) { + v[p] = vbus; + r_eff[p] += rds; + conducting[p] = true; + } else if (lo[p]) { + v[p] = 0; + r_eff[p] += rds; + conducting[p] = true; + } else if (fabs(m.i[p]) > i_eps) { + // body diode freewheeling (paper eq 7, current condition) + v[p] = m.i[p] < 0 ? vbus + vf : -vf; + conducting[p] = true; + } else { + m.i[p] = 0; + v[p] = 0; // filled in after v_star is known + conducting[p] = false; + } + } + + // star point voltage from the conducting phases (paper eq 10; sum of + // currents is zero and impedances are equal). With nothing conducting + // the network floats; centre it on the bus as a symmetric bridge does + double v_star = 0; + int n_cond = 0; + for (int p = 0; p < 3; p++) { + if (conducting[p]) { + v_star += v[p] - e[p]; + n_cond++; + } + } + v_star = n_cond > 0 ? v_star / n_cond : 0.5 * vbus; + + // paper eq 7 second condition / fig 2(e): a body diode also turns on + // when the floating terminal voltage e + v_m would exceed the rails, + // even with no phase current. This clamps the floating phase at high + // BEMF and lets a windmilling motor rectify into the battery. Each + // new clamp moves the star point, so iterate + for (int pass = 0; pass < 3; pass++) { + bool changed = false; + for (int p = 0; p < 3; p++) { + if (conducting[p]) { + continue; + } + const double vt = e[p] + v_star; + if (vt > vbus + vf) { + v[p] = vbus + vf; + } else if (vt < -vf) { + v[p] = -vf; + } else { + continue; + } + conducting[p] = true; + changed = true; + } + if (!changed) { + break; + } + v_star = 0; + n_cond = 0; + for (int p = 0; p < 3; p++) { + if (conducting[p]) { + v_star += v[p] - e[p]; + n_cond++; + } + } + v_star /= n_cond; + } + + // an open phase floats at e + v_m (paper eq 9) + for (int p = 0; p < 3; p++) { + if (!conducting[p]) { + v[p] = e[p] + v_star; + } + } + + for (int p = 0; p < 3; p++) { + m.v_term[p] = v[p]; + } + + // phase current derivatives (paper eq 1), forward euler + for (int p = 0; p < 3; p++) { + if (!conducting[p]) { + continue; + } + const double di = (v[p] - r_eff[p] * m.i[p] - e[p] - v_star) / L_eff; + m.i[p] += di * dt; + } + // with exactly two conducting phases KCL forces them equal and + // opposite; project out any numerical drift + if (n_cond == 2) { + int a = -1, b = -1; + for (int p = 0; p < 3; p++) { + if (conducting[p]) { + if (a < 0) { + a = p; + } else { + b = p; + } + } + } + const double ic = 0.5 * (m.i[a] - m.i[b]); + m.i[a] = ic; + m.i[b] = -ic; + } else if (n_cond < 2) { + for (int p = 0; p < 3; p++) { + m.i[p] = 0; + } + } + + // electromagnetic torque: tau = ke * sum(shape_p * i_p) (paper eq 2) + double tau = 0; + for (int p = 0; p < 3; p++) { + tau += m.ke * shape[p] * m.i[p]; + } + + // load torques and motion (paper eq 3, plus k*omega^2 propeller + // load and static friction) + const double w = m.omega; + double tau_load = sitl_cfg.motor.damping * w + sitl_cfg.motor.load_k_omega2 * w * fabs(w); + double tau_net = tau - tau_load; + const double sf = sitl_cfg.motor.static_friction; + if (fabs(w) < 0.5) { + // static friction dead band + if (fabs(tau_net) <= sf) { + tau_net = 0; + m.omega = 0; + } else { + tau_net -= (tau_net > 0 ? sf : -sf); + } + } else { + tau_net -= (w > 0 ? sf : -sf); + } + m.omega += tau_net / sitl_cfg.motor.inertia * dt; + m.theta += m.omega * dt; + if (m.theta > TWO_PI || m.theta < -TWO_PI) { + m.theta = fmod(m.theta, TWO_PI); + } + + // battery current: everything sourced from the positive rail + double ibus = 0; + for (int p = 0; p < 3; p++) { + if (conducting[p] && v[p] >= vbus - 1e-9) { + ibus += m.i[p]; + } + } + m.ibus = ibus; + + // comparator: virtual neutral against the floating phase terminal + const double v_neutral = (v[0] + v[1] + v[2]) / 3.0; + const double v_float = v[sitl_comp_phase]; + double diff_mv = (v_neutral - v_float) * 1000.0; + if (sitl_cfg.sim.comparator_noise_mv > 0) { + const double r = (double)rand_r(&m.rand_seed) / RAND_MAX - 0.5; + diff_mv += r * 2.0 * sitl_cfg.sim.comparator_noise_mv; + } + const double hyst = sitl_cfg.sim.comparator_hysteresis_mv * 0.5; + uint8_t out = sitl_comp_out; + if (out) { + out = diff_mv > -hyst; + } else { + out = diff_mv > hyst; + } + if (out != sitl_comp_out) { + sitl_comp_out = out; + const uint32_t line = current_EXTI_LINE; + const bool rising_edge = out != 0; + if ((rising_edge && (sitl_exti.RTSR & line)) || (!rising_edge && (sitl_exti.FTSR & line))) { + sitl_exti.PR |= line; + const bool unmasked = (sitl_exti.IMR & line) != 0; + if (unmasked) { + sitl_irq_pend(SITL_IRQ_COMP); + } + motor_log_event(MEV_EDGE, out, unmasked, 0); + } + } + + check_desync_dump(); + + // sensor averaging, snapshot every 64 steps + m.acc_v += m.vbus; + m.acc_i += ibus > 0 ? ibus : 0; + m.acc_n++; + if (m.acc_n >= 64) { + sitl_sensors_t s; + s.bus_voltage = (float)(m.acc_v / m.acc_n); + s.bus_current = (float)(m.acc_i / m.acc_n); + s.temperature_c = sitl_cfg.esc.temperature_c; + s.rpm = (float)(m.omega * 60.0 / TWO_PI); + sitl_sensors_write(&s); + m.acc_v = m.acc_i = 0; + m.acc_n = 0; + } +} + +/* + commutation debug ring: comStep calls in here; on a firmware desync the + recent history is dumped so the failure can be analysed + */ +#define COMM_RING 96 +static struct { + uint64_t t_ns; + float thetae_deg; // electrical angle + float rpm; + uint32_t ci; + uint32_t a, b, c; + uint8_t kind; +} comm_ring[COMM_RING]; +static unsigned comm_ring_pos; + +void motor_log_event(int kind, uint32_t a, uint32_t b, uint32_t c) +{ + extern volatile uint32_t commutation_interval; + const int pole_pairs = sitl_cfg.motor.poles / 2; + double deg = fmod(m.theta * pole_pairs * 180.0 / M_PI, 360.0); + if (deg < 0) { + deg += 360; + } + const unsigned idx = __atomic_fetch_add(&comm_ring_pos, 1, __ATOMIC_SEQ_CST) % COMM_RING; + comm_ring[idx].t_ns = sitl_time_ns(); + comm_ring[idx].thetae_deg = (float)deg; + comm_ring[idx].rpm = (float)(m.omega * 60.0 / TWO_PI); + comm_ring[idx].ci = commutation_interval; + comm_ring[idx].a = a; + comm_ring[idx].b = b; + comm_ring[idx].c = c; + comm_ring[idx].kind = (uint8_t)kind; + +} + +void motor_log_mainloop(void) +{ + extern volatile uint32_t average_interval; + extern uint32_t last_average_interval; + static uint64_t last_ns; + const uint64_t now = sitl_time_ns(); + const uint32_t gap_us = (uint32_t)((now - last_ns) / 1000ULL); + last_ns = now; + // only log iterations after a gap to avoid flooding the ring + if (gap_us >= 50) { + motor_log_event(MEV_MAINLOOP, average_interval, last_average_interval, gap_us); + } +} + +void motor_log_commutation(int step) +{ + extern volatile uint16_t duty_cycle; + extern uint16_t duty_cycle_maximum; + motor_log_event(MEV_COMMUTATE, (uint32_t)step, duty_cycle, duty_cycle_maximum); +} + +static void check_desync_dump(void) +{ + extern uint32_t desync_happened; + static uint32_t last_desync; + static int dumps; + if (desync_happened == last_desync) { + return; + } + last_desync = desync_happened; + if (dumps >= 3) { + return; + } + dumps++; + extern volatile uint32_t average_interval; + extern uint32_t last_average_interval; + extern volatile uint32_t zero_crosses; + extern int e_com_time; + fprintf(stderr, + "SITL: desync %u at t=%.4fs avg=%u last_avg=%u zc=%u e_com_time=%d, recent events:\n", + (unsigned)desync_happened, sitl_time_ns() * 1e-9, + (unsigned)average_interval, (unsigned)last_average_interval, + (unsigned)zero_crosses, e_com_time); + dump_ring(); +} + +static void dump_ring(void) +{ + static const char* kinds[] = { "COMMUTATE", "EDGE", "BLANKED", "COMP_RUN", "ZC_ACCEPT", "MAINLOOP" }; + for (unsigned k = 0; k < COMM_RING; k++) { + const unsigned idx = (comm_ring_pos + k) % COMM_RING; + if (comm_ring[idx].t_ns == 0) { + continue; + } + fprintf(stderr, " t=%.6f %-9s thetae=%5.1f rpm=%6.0f ci=%u a=%u b=%u c=%u\n", + comm_ring[idx].t_ns * 1e-9, kinds[comm_ring[idx].kind], + (double)comm_ring[idx].thetae_deg, (double)comm_ring[idx].rpm, + (unsigned)comm_ring[idx].ci, + (unsigned)comm_ring[idx].a, (unsigned)comm_ring[idx].b, + (unsigned)comm_ring[idx].c); + } +} + +void motor_get_state(double* theta, double* omega, double i[3]) +{ + *theta = m.theta; + *omega = m.omega; + for (int p = 0; p < 3; p++) { + i[p] = m.i[p]; + } +} + +void motor_print_state(uint64_t now_ns, float time_ratio) +{ + // firmware state, for debug output only + extern uint16_t input; + extern volatile char armed; + extern char step; + extern volatile uint32_t zero_crosses; + extern volatile uint32_t commutation_interval; + extern volatile uint16_t duty_cycle; + extern uint8_t bemf_timeout_happened; + extern uint8_t running; + extern char old_routine; + extern volatile uint16_t newinput; + extern uint16_t adjusted_input; + extern EEprom_t eepromBuffer; + static bool printed_settings; + if (!printed_settings) { + printed_settings = true; + fprintf(stderr, + "SITL settings: bidir=%u dir_rev=%u comp_pwm=%u poles=%u input_type=%u sine=%u brake_on_stop=%u\n", + eepromBuffer.bi_direction, eepromBuffer.dir_reversed, + eepromBuffer.comp_pwm, eepromBuffer.motor_poles, + eepromBuffer.input_type, eepromBuffer.use_sine_start, + eepromBuffer.brake_on_stop); + } + + extern void sitl_can_stats(uint32_t stats[4]); + uint32_t cs[4]; + sitl_can_stats(cs); + + fprintf(stderr, + "SITL t=%.1fs x%.2f rpm=%.0f Vbus=%.2f Ibus=%.2f modes=%d%d%d in=%u newin=%u adj=%u armed=%d duty=%u step=%d zc=%u ci=%u run=%d old=%d bemf_to=%u cmd=%u\n", + now_ns * 1e-9, (double)time_ratio, m.omega * 60.0 / TWO_PI, + m.vbus, m.ibus, + sitl_phase_mode[0], sitl_phase_mode[1], sitl_phase_mode[2], + input, newinput, adjusted_input, armed, duty_cycle, step, + (unsigned)zero_crosses, (unsigned)commutation_interval, + running, old_routine, bemf_timeout_happened, + (unsigned)cs[1]); +} diff --git a/Mcu/SITL/sim/motor.h b/Mcu/SITL/sim/motor.h new file mode 100644 index 000000000..94f2a6851 --- /dev/null +++ b/Mcu/SITL/sim/motor.h @@ -0,0 +1,46 @@ +/* + motor.h - BLDC motor, bridge and battery simulation for AM32 SITL + */ + +#pragma once + +#include + +void motor_init(void); + +// advance the electrical/mechanical model by dt_ns. Called from the sim +// thread only +void motor_step(uint64_t now_ns, uint32_t dt_ns); + +// 1Hz state line on stderr for --verbose +void motor_print_state(uint64_t now_ns, float time_ratio); + +// direct state access for offline tests +void motor_get_state(double* theta, double* omega, double i[3]); + +// snapshot for the state streaming port (sitl_state.c), sim thread only +void motor_get_live_state(float* omega, float* theta, float* theta_e, + float i[3], float v[3], float* vbus, float* ibus); + +// re-derive cached values after a runtime config reload +void motor_config_changed(void); + +// accumulate iu,iv,iw,vu,vv,vw,vbus,ibus for averaged state sampling +void motor_add_signals(double acc[8]); + +// called from the firmware main loop for debug tracing +void motor_log_mainloop(void); + +// commutation debug logging, called from comStep +void motor_log_commutation(int step); + +// generic event logging into the same debug ring +enum motor_ev { + MEV_COMMUTATE = 0, // a = step + MEV_EDGE, // comparator edge: a = new level, b = IMR set, c = pended + MEV_COMP_BLANKED, // comp irq discarded by blanking: a = CNT, b = ci/2 + MEV_COMP_RUN, // comp irq calling interruptRoutine: a = CNT + MEV_ZC_ACCEPT, // interruptRoutine armed COM timer: a = waitTime + MEV_MAINLOOP, // firmware main loop iteration: a = avg, b = last_avg, c = gap us +}; +void motor_log_event(int kind, uint32_t a, uint32_t b, uint32_t c); diff --git a/Mcu/SITL/sim/sitl_config.c b/Mcu/SITL/sim/sitl_config.c new file mode 100644 index 000000000..d88e998f7 --- /dev/null +++ b/Mcu/SITL/sim/sitl_config.c @@ -0,0 +1,392 @@ +/* + sitl_config.c - JSON config file and command line handling for AM32 SITL + */ + +#include "sitl_config.h" + +#include +#include +#include +#include +#include + +#include "jsmn.h" + +sitl_config_t sitl_cfg = { + .motor = { + .kv = 900, + .poles = 14, + .resistance = 0.045f, + .inductance = 2.1e-5f, + .mutual_inductance = 0.0f, + // defaults model a ~7 inch prop on a 900Kv motor at 4S: about + // 19A and 13500 rpm at full throttle. Rotors much lighter than + // this accelerate faster than the firmware desync detection + // allows + .inertia = 3.0e-5f, + .damping = 1.0e-6f, + .static_friction = 0.003f, + .load_k_omega2 = 1.0e-7f, + }, + .battery = { + .voltage = 16.8f, + .resistance = 0.012f, + }, + .esc = { + .rds_on = 0.004f, + .diode_vf = 0.7f, + .temperature_c = 25.0f, + }, + .sim = { + .physics_dt_ns = 500, + .loop_time_ns = 2000, + .isr_read_ns = 100, + .comparator_noise_mv = 5.0f, + // enough hysteresis that noise cannot eat a zero crossing edge, + // as on a real comparator + .comparator_hysteresis_mv = 15.0f, + .watchdog_enabled = true, + }, + .speedup = 1.0f, + .input_port = 57733, + .state_port = 57734, + .bind_any = false, + .eeprom_path = "am32_eeprom.bin", + .can_uri = "mcast:0", + .uid = NULL, + .node_id = -1, + .input_type = -1, + .verbose = false, + .nosleep = false, + .realtime = false, +}; + +struct cfg_entry { + const char* section; + const char* key; + enum { CFG_FLOAT, + CFG_INT, + CFG_U32, + CFG_BOOL } type; + void* ptr; +}; + +static const struct cfg_entry cfg_table[] = { + { "motor", "kv", CFG_FLOAT, &sitl_cfg.motor.kv }, + { "motor", "poles", CFG_INT, &sitl_cfg.motor.poles }, + { "motor", "resistance", CFG_FLOAT, &sitl_cfg.motor.resistance }, + { "motor", "inductance", CFG_FLOAT, &sitl_cfg.motor.inductance }, + { "motor", "mutual_inductance", CFG_FLOAT, &sitl_cfg.motor.mutual_inductance }, + { "motor", "inertia", CFG_FLOAT, &sitl_cfg.motor.inertia }, + { "motor", "damping", CFG_FLOAT, &sitl_cfg.motor.damping }, + { "motor", "static_friction", CFG_FLOAT, &sitl_cfg.motor.static_friction }, + { "motor", "load_k_omega2", CFG_FLOAT, &sitl_cfg.motor.load_k_omega2 }, + { "battery", "voltage", CFG_FLOAT, &sitl_cfg.battery.voltage }, + { "battery", "resistance", CFG_FLOAT, &sitl_cfg.battery.resistance }, + { "esc", "rds_on", CFG_FLOAT, &sitl_cfg.esc.rds_on }, + { "esc", "diode_vf", CFG_FLOAT, &sitl_cfg.esc.diode_vf }, + { "esc", "temperature_c", CFG_FLOAT, &sitl_cfg.esc.temperature_c }, + { "sim", "physics_dt_ns", CFG_U32, &sitl_cfg.sim.physics_dt_ns }, + { "sim", "loop_time_ns", CFG_U32, &sitl_cfg.sim.loop_time_ns }, + { "sim", "isr_read_ns", CFG_U32, &sitl_cfg.sim.isr_read_ns }, + { "sim", "comparator_noise_mv", CFG_FLOAT, &sitl_cfg.sim.comparator_noise_mv }, + { "sim", "comparator_hysteresis_mv", CFG_FLOAT, &sitl_cfg.sim.comparator_hysteresis_mv }, + { "sim", "watchdog_enabled", CFG_BOOL, &sitl_cfg.sim.watchdog_enabled }, +}; + +static bool set_value(const char* section, const char* js, const jsmntok_t* key, const jsmntok_t* val, const char* path) +{ + char keystr[64], valstr[64]; + snprintf(keystr, sizeof(keystr), "%.*s", key->end - key->start, js + key->start); + snprintf(valstr, sizeof(valstr), "%.*s", val->end - val->start, js + val->start); + for (unsigned i = 0; i < sizeof(cfg_table) / sizeof(cfg_table[0]); i++) { + const struct cfg_entry* e = &cfg_table[i]; + if (strcmp(e->section, section) != 0 || strcmp(e->key, keystr) != 0) { + continue; + } + switch (e->type) { + case CFG_FLOAT: + *(float*)e->ptr = strtof(valstr, NULL); + break; + case CFG_INT: + *(int*)e->ptr = atoi(valstr); + break; + case CFG_U32: + *(uint32_t*)e->ptr = strtoul(valstr, NULL, 0); + break; + case CFG_BOOL: + *(bool*)e->ptr = (strcmp(valstr, "true") == 0 || strcmp(valstr, "1") == 0); + break; + } + return true; + } + fprintf(stderr, "SITL: %s: unknown config key %s.%s\n", path, section, keystr); + return false; +} + +// count the tokens making up one JSON value, for skipping +static int value_size(const jsmntok_t* t) +{ + int count = 1; + if (t->type == JSMN_OBJECT) { + const jsmntok_t* p = t + 1; + for (int i = 0; i < t->size; i++) { + count++; // key + const int vs = value_size(p + 1); + count += vs; + p += 1 + vs; + } + } else if (t->type == JSMN_ARRAY) { + const jsmntok_t* p = t + 1; + for (int i = 0; i < t->size; i++) { + const int vs = value_size(p); + count += vs; + p += vs; + } + } + return count; +} + +/* + load a JSON config file. When runtime is true only the motor, battery + and esc sections are applied (the sim section cannot change while + running) and errors are reported instead of exiting + */ +static bool load_json_ex(const char* path, bool runtime) +{ + FILE* f = fopen(path, "r"); + if (!f) { + fprintf(stderr, "SITL: failed to open config %s\n", path); + return false; + } + static char js[16384]; + const size_t n = fread(js, 1, sizeof(js) - 1, f); + fclose(f); + js[n] = 0; + + jsmn_parser parser; + static jsmntok_t tokens[512]; + jsmn_init(&parser); + const int ntok = jsmn_parse(&parser, js, n, tokens, 512); + if (ntok < 1 || tokens[0].type != JSMN_OBJECT) { + fprintf(stderr, "SITL: invalid JSON in %s (err %d)\n", path, ntok); + return false; + } + + bool ok = true; + const jsmntok_t* t = &tokens[1]; + for (int i = 0; i < tokens[0].size; i++) { + const jsmntok_t* section = t; + const jsmntok_t* sec_obj = t + 1; + char secstr[64]; + snprintf(secstr, sizeof(secstr), "%.*s", section->end - section->start, js + section->start); + if (sec_obj->type != JSMN_OBJECT) { + fprintf(stderr, "SITL: %s: section %s is not an object\n", path, secstr); + return false; + } + const bool skip = runtime && strcmp(secstr, "motor") != 0 && + strcmp(secstr, "battery") != 0 && strcmp(secstr, "esc") != 0; + const jsmntok_t* kt = sec_obj + 1; + for (int k = 0; k < sec_obj->size; k++) { + const jsmntok_t* val = kt + 1; + if (!skip && !set_value(secstr, js, kt, val, path)) { + ok = false; + } + kt += 1 + value_size(val); + } + t += 1 + value_size(sec_obj); + } + return ok; +} + +static void load_json(const char* path) +{ + if (!load_json_ex(path, false)) { + exit(1); + } +} + +/* + reject config values that would break the physics: motor.c divides by + kv, inertia and L - M, and any NaN poisons the whole state. Clamp with + a warning rather than exit so a bad runtime LOAD_MODEL cannot wedge a + running simulation + */ +static void clampf(float* v, float lo, float hi, const char* name) +{ + float nv = *v; + if (!isfinite(nv) || nv < lo) { + nv = lo; + } else if (nv > hi) { + nv = hi; + } + if (nv != *v || !isfinite(*v)) { + fprintf(stderr, "SITL: config %s=%g clamped to %g\n", name, (double)*v, (double)nv); + *v = nv; + } +} + +static void clampu32(uint32_t* v, uint32_t lo, uint32_t hi, const char* name) +{ + uint32_t nv = *v; + if (nv < lo) { + nv = lo; + } else if (nv > hi) { + nv = hi; + } + if (nv != *v) { + fprintf(stderr, "SITL: config %s=%u clamped to %u\n", name, *v, nv); + *v = nv; + } +} + +static void config_sanitise(void) +{ + clampf(&sitl_cfg.motor.kv, 1.0f, 1e6f, "motor.kv"); + if (sitl_cfg.motor.poles < 2 || (sitl_cfg.motor.poles & 1)) { + fprintf(stderr, "SITL: config motor.poles=%d invalid, using 2\n", sitl_cfg.motor.poles); + sitl_cfg.motor.poles = sitl_cfg.motor.poles > 2 ? sitl_cfg.motor.poles & ~1 : 2; + } + clampf(&sitl_cfg.motor.resistance, 1e-4f, 100.0f, "motor.resistance"); + clampf(&sitl_cfg.motor.inductance, 1e-8f, 1.0f, "motor.inductance"); + // keep L - M positive + clampf(&sitl_cfg.motor.mutual_inductance, -1.0f, + sitl_cfg.motor.inductance * 0.9f, "motor.mutual_inductance"); + clampf(&sitl_cfg.motor.inertia, 1e-9f, 100.0f, "motor.inertia"); + clampf(&sitl_cfg.motor.damping, 0.0f, 1.0f, "motor.damping"); + clampf(&sitl_cfg.motor.static_friction, 0.0f, 10.0f, "motor.static_friction"); + clampf(&sitl_cfg.motor.load_k_omega2, 0.0f, 1.0f, "motor.load_k_omega2"); + clampf(&sitl_cfg.battery.voltage, 1.0f, 200.0f, "battery.voltage"); + clampf(&sitl_cfg.battery.resistance, 0.0f, 10.0f, "battery.resistance"); + clampf(&sitl_cfg.esc.rds_on, 0.0f, 1.0f, "esc.rds_on"); + clampf(&sitl_cfg.esc.diode_vf, 0.0f, 5.0f, "esc.diode_vf"); + + // physics_dt_ns == 0 turns sitl_isr_read_tick() into an infinite loop + // (accum_ns -= 0). Keep steps in a sane range for the BLDC model. + clampu32(&sitl_cfg.sim.physics_dt_ns, 100U, 10000U, "sim.physics_dt_ns"); + // isr_read_ns == 0 freezes progress under PRIMASK (startup tunes / + // micros64 critical sections never complete). Cap at a few physics steps. + clampu32(&sitl_cfg.sim.isr_read_ns, 1U, sitl_cfg.sim.physics_dt_ns * 10U, "sim.isr_read_ns"); + clampu32(&sitl_cfg.sim.loop_time_ns, 100U, 1000000U, "sim.loop_time_ns"); + + // match SET_SPEEDUP on the state port: [0, 100], 0 = free run + if (!isfinite(sitl_cfg.speedup) || sitl_cfg.speedup < 0.0f || sitl_cfg.speedup > 100.0f) { + const float nv = (!isfinite(sitl_cfg.speedup) || sitl_cfg.speedup < 0.0f) ? 0.0f : 100.0f; + fprintf(stderr, "SITL: config speedup=%g clamped to %g\n", + (double)sitl_cfg.speedup, (double)nv); + sitl_cfg.speedup = nv; + } +} + +bool sitl_config_reload(const char* path) +{ + const bool ok = load_json_ex(path, true); + config_sanitise(); + return ok; +} + +static void usage(const char* prog) +{ + printf("Usage: %s [options]\n" + " --config FILE JSON config file for motor/battery/esc/sim\n" + " --eeprom FILE eeprom backing file (default am32_eeprom.bin)\n" + " --can-uri URI CAN interface (default mcast:0)\n" + " --input-port N UDP port for PWM/DShot input, 0 to disable\n" + " (default 57733)\n" + " --state-port N UDP port for simulation state streaming and\n" + " runtime model control, 0 to disable\n" + " (default 57734)\n" + " --speedup X simulation speed, 0 for free running (default 1.0)\n" + " --bind-any bind the input/state UDP ports on all interfaces\n" + " instead of loopback only, for a GUI on another\n" + " host\n" + " --node-id N force DroneCAN node ID\n" + " --input-type N force eeprom INPUT_SIGNAL_TYPE (0=auto 1=dshot\n" + " 2=servo 5=dronecan)\n" + " --uid STR string used to derive the 16 byte unique ID\n" + " --verbose 1Hz state output on stderr\n" + " --nosleep busy wait instead of sleeping (uses two full\n" + " CPU cores but gives the most accurate timing)\n" + " --realtime SCHED_FIFO scheduling for both threads (needs\n" + " root or an rtprio rlimit; with --nosleep also\n" + " set kernel.sched_rt_runtime_us=-1)\n", + prog); +} + +void sitl_config_init(int argc, char** argv) +{ + static const struct option opts[] = { + { "config", required_argument, NULL, 'c' }, + { "eeprom", required_argument, NULL, 'e' }, + { "can-uri", required_argument, NULL, 'u' }, + { "input-port", required_argument, NULL, 'p' }, + { "state-port", required_argument, NULL, 'P' }, + { "speedup", required_argument, NULL, 's' }, + { "bind-any", no_argument, NULL, 'A' }, + { "node-id", required_argument, NULL, 'n' }, + { "input-type", required_argument, NULL, 'I' }, + { "uid", required_argument, NULL, 'U' }, + { "verbose", no_argument, NULL, 'v' }, + { "nosleep", no_argument, NULL, 'N' }, + { "realtime", no_argument, NULL, 'R' }, + { "help", no_argument, NULL, 'h' }, + { NULL, 0, NULL, 0 }, + }; + int c; + while ((c = getopt_long(argc, argv, "c:e:u:p:P:s:An:I:U:vNRh", opts, NULL)) != -1) { + switch (c) { + case 'c': + load_json(optarg); + break; + case 'e': + sitl_cfg.eeprom_path = optarg; + break; + case 'u': + sitl_cfg.can_uri = optarg; + break; + case 'p': + sitl_cfg.input_port = atoi(optarg); + break; + case 'P': + sitl_cfg.state_port = atoi(optarg); + break; + case 's': { + char* end = NULL; + const float v = strtof(optarg, &end); + if (end == optarg || (end && *end != '\0') || !isfinite(v)) { + fprintf(stderr, "SITL: invalid --speedup '%s' (need a finite number)\n", optarg); + exit(1); + } + // range clamped in config_sanitise() to [0, 100] + sitl_cfg.speedup = v; + break; + } + case 'A': + sitl_cfg.bind_any = true; + break; + case 'n': + sitl_cfg.node_id = atoi(optarg); + break; + case 'I': + sitl_cfg.input_type = atoi(optarg); + break; + case 'U': + sitl_cfg.uid = optarg; + break; + case 'v': + sitl_cfg.verbose = true; + break; + case 'N': + sitl_cfg.nosleep = true; + break; + case 'R': + sitl_cfg.realtime = true; + break; + case 'h': + default: + usage(argv[0]); + exit(c == 'h' ? 0 : 1); + } + } + config_sanitise(); +} diff --git a/Mcu/SITL/sim/sitl_config.h b/Mcu/SITL/sim/sitl_config.h new file mode 100644 index 000000000..7370d0db4 --- /dev/null +++ b/Mcu/SITL/sim/sitl_config.h @@ -0,0 +1,62 @@ +/* + sitl_config.h - JSON file + command line configuration for AM32 SITL + */ + +#pragma once + +#include +#include + +typedef struct { + struct { + float kv; // rpm per volt + int poles; // magnetic poles (not pole pairs) + float resistance; // phase resistance, ohm + float inductance; // phase self inductance, henry + float mutual_inductance; // henry (negative or zero) + float inertia; // rotor+prop inertia, kg m^2 + float damping; // Nm/(rad/s) + float static_friction; // Nm + float load_k_omega2; // propeller load: Nm/(rad/s)^2 + } motor; + struct { + float voltage; // open circuit volts + float resistance; // internal resistance, ohm + } battery; + struct { + float rds_on; // fet on resistance, ohm + float diode_vf; // body diode forward voltage + float temperature_c; // reported temperature + } esc; + struct { + uint32_t physics_dt_ns; // integration step + uint32_t loop_time_ns; // firmware main loop pacing sleep + uint32_t isr_read_ns; // cost of a register read in interrupt context + float comparator_noise_mv; + float comparator_hysteresis_mv; + bool watchdog_enabled; + } sim; + + // runtime options + float speedup; // 0 = free run + int input_port; // UDP port for PWM/DShot input, 0 disables + int state_port; // UDP port for state streaming/model control, 0 disables + bool bind_any; // bind input/state ports on all interfaces, not loopback + const char* eeprom_path; + const char* can_uri; + const char* uid; // optional fixed unique ID string + int node_id; // -1 = leave to eeprom/DNA + int input_type; // eeprom INPUT_SIGNAL_TYPE override, -1 = leave + bool verbose; + bool nosleep; // busy wait instead of sleeping, for timing accuracy + bool realtime; // SCHED_FIFO for both threads +} sitl_config_t; + +extern sitl_config_t sitl_cfg; + +// parse CLI and optional JSON config, exits on error +void sitl_config_init(int argc, char** argv); + +// runtime reload of the motor/battery/esc sections from a JSON file +// (sim section ignored). Returns false on error, never exits +bool sitl_config_reload(const char* path); diff --git a/Mcu/SITL/sitl_can_test.py b/Mcu/SITL/sitl_can_test.py new file mode 100644 index 000000000..2eaea6aac --- /dev/null +++ b/Mcu/SITL/sitl_can_test.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 +''' +test AM32 SITL over DroneCAN multicast UDP + +runs a throttle ramp via esc.RawCommand and reports esc.Status +telemetry. Needs the SITL binary already running on the same mcast bus, +eg: + obj/AM32_AM32_SITL_CAN_*.elf --node-id 10 --verbose + python3 Mcu/SITL/sitl_can_test.py --throttle 0.5 --duration 20 +''' + +import argparse +import time + +import dronecan + +parser = argparse.ArgumentParser(description=__doc__) +parser.add_argument('--uri', default='mcast:0') +parser.add_argument('--node-id', type=int, default=100, help='our node ID') +parser.add_argument('--esc-index', type=int, default=0) +parser.add_argument('--throttle', type=float, default=0.5, help='peak throttle 0..1') +parser.add_argument('--ramp-time', type=float, default=3.0, help='seconds to reach peak') +parser.add_argument('--duration', type=float, default=15.0) +parser.add_argument('--rate', type=float, default=50.0, help='RawCommand rate Hz') +args = parser.parse_args() + + +def main(): + node = dronecan.make_node(args.uri, node_id=args.node_id, bitrate=1000000) + + state = {'nodes': set(), 'status': None, 'nstatus': 0} + + def on_node_status(e): + nid = e.transfer.source_node_id + if nid not in state['nodes']: + state['nodes'].add(nid) + print('found node %u (mode %u health %u)' % ( + nid, e.message.mode, e.message.health)) + + def on_esc_status(e): + state['status'] = e.message + state['nstatus'] += 1 + + node.add_handler(dronecan.uavcan.protocol.NodeStatus, on_node_status) + node.add_handler(dronecan.uavcan.equipment.esc.Status, on_esc_status) + + # wait for the ESC to appear + deadline = time.time() + 10 + while not state['nodes'] and time.time() < deadline: + node.spin(0.1) + if not state['nodes']: + print('FAIL: no DroneCAN node seen') + return 1 + + print('arming and ramping to %.0f%% throttle' % (args.throttle * 100)) + t0 = time.time() + next_cmd = t0 + last_print = t0 + while time.time() - t0 < args.duration: + node.spin(0) + now = time.time() + if now >= next_cmd: + next_cmd += 1.0 / args.rate + # arming plus raw command + arming = dronecan.uavcan.equipment.safety.ArmingStatus( + status=dronecan.uavcan.equipment.safety.ArmingStatus().STATUS_FULLY_ARMED) + node.broadcast(arming) + ramp = min(1.0, (now - t0) / args.ramp_time) + value = int(8191 * args.throttle * ramp) + commands = [0] * (args.esc_index + 1) + commands[args.esc_index] = value + node.broadcast(dronecan.uavcan.equipment.esc.RawCommand(cmd=commands)) + if now - last_print >= 1.0 and state['status'] is not None: + last_print = now + s = state['status'] + print('rpm=%6d volts=%.2f amps=%.2f temp=%.0fC err=%u (%u pkts)' % ( + s.rpm, s.voltage, s.current, + s.temperature - 273.15, s.error_count, state['nstatus'])) + time.sleep(0.001) + + # stop the motor + for _ in range(10): + node.broadcast(dronecan.uavcan.equipment.esc.RawCommand(cmd=[0])) + node.spin(0.02) + + if state['nstatus'] == 0: + print('FAIL: no esc.Status telemetry received') + return 1 + s = state['status'] + print('final: rpm=%d volts=%.2f amps=%.2f' % (s.rpm, s.voltage, s.current)) + return 0 + + +if __name__ == '__main__': + exit(main()) diff --git a/Mcu/SITL/sitl_dshot.py b/Mcu/SITL/sitl_dshot.py new file mode 100644 index 000000000..8059599d5 --- /dev/null +++ b/Mcu/SITL/sitl_dshot.py @@ -0,0 +1,250 @@ +''' +protocol library for AM32 SITL PWM/DShot input over UDP + +packet format (little endian): + u16 magic 0x4453 + u8 type: 0=PWM, 1=DSHOT150, 2=DSHOT300, 3=DSHOT600 + u8 len: payload bytes after the 6 byte header (4) + u16 flags: bit0 = line idle level (1 = idle high, inverted/bidir DShot) + u16 data: PWM pulse width in microseconds, or the full 16 bit DShot + frame (11 bit value, telemetry bit, 4 bit CRC) + +replies (bidirectional DShot) use the same format with data being the 16 +bit GCR-decoded frame: 12 bit eRPM/EDT value plus 4 bit CRC +''' + +import socket +import struct +import time +import threading + +MAGIC = 0x4453 +TYPE_PWM = 0 +TYPE_DSHOT150 = 1 +TYPE_DSHOT300 = 2 +TYPE_DSHOT600 = 3 +FLAG_IDLE_HIGH = 0x0001 + +TYPE_NAMES = { + 'pwm': TYPE_PWM, + 'dshot150': TYPE_DSHOT150, + 'dshot300': TYPE_DSHOT300, + 'dshot600': TYPE_DSHOT600, +} + +# DShot commands (value field 1..47 at zero throttle) +DSHOT_CMD_EDT_ENABLE = 13 +DSHOT_CMD_EDT_DISABLE = 14 + + +def dshot_crc(value12, bidir=False): + '''4 bit CRC over the 12 bit value+telemetry field''' + csum = value12 ^ (value12 >> 4) ^ (value12 >> 8) + if bidir: + csum = ~csum + return csum & 0xF + + +def dshot_frame(value11, telem=False, bidir=False, corrupt=False): + '''compose the full 16 bit DShot frame from an 11 bit value''' + v = ((value11 & 0x7FF) << 1) | (1 if telem else 0) + crc = dshot_crc(v, bidir) + if corrupt: + crc ^= 0x5 + return (v << 4) | crc + + +def check_reply_crc(frame16): + '''BDShot reply CRC: xor of the four nibbles must be 0xF''' + n = frame16 + return ((n ^ (n >> 4) ^ (n >> 8) ^ (n >> 12)) & 0xF) == 0xF + + +def decode_reply(frame16, edt_expected=False): + '''decode a 16 bit BDShot reply frame. + returns (kind, value) where kind is one of: + 'erpm' - value is the eRPM period in microseconds (65408 = stopped) + 'temp' - degrees C + 'volt' - volts + 'current' - amps + 'edt' - other extended frame, value is the raw 12 bits + 'badcrc' - CRC failure, value is the raw frame + ''' + if not check_reply_crc(frame16): + return ('badcrc', frame16) + val = frame16 >> 4 + # extended telemetry frames have bit 8 clear (eRPM mantissa is + # normalised so real eRPM frames have it set) + if edt_expected and (val & 0x100) == 0 and val != 0: + etype = val >> 8 + data = val & 0xFF + if etype == 0x2: + return ('temp', data) + if etype == 0x4: + return ('volt', data * 0.25) + if etype == 0x6: + # AM32 encodes centiamps/50, ie 0.5A units + return ('current', data * 0.5) + return ('edt', val) + period_us = (val & 0x1FF) << (val >> 9) + return ('erpm', period_us) + + +def erpm_period_to_rpm(period_us, motor_poles=14): + '''convert eRPM period in us to mechanical RPM''' + if period_us == 0 or period_us >= 65408: + return 0.0 + erpm = 60.0e6 / period_us + return erpm / (motor_poles / 2) + + +def pack(ptype, flags, data): + return struct.pack(' 1000: + self.replies = self.replies[-500:] + + def close(self): + self.running = False + self.sock.close() + + def send_pwm(self, width_us, idle_high=False): + flags = FLAG_IDLE_HIGH if idle_high else 0 + self.sock.sendto(pack(TYPE_PWM, flags, width_us), self.addr) + self.sent_count += 1 + + def send_dshot(self, value11, ptype=TYPE_DSHOT300, telem=False, + bidir=False, corrupt=False): + flags = FLAG_IDLE_HIGH if bidir else 0 + frame = dshot_frame(value11, telem=telem, bidir=bidir, corrupt=corrupt) + self.sock.sendto(pack(ptype, flags, frame), self.addr) + self.sent_count += 1 + + def get_replies(self): + with self.lock: + r = self.replies + self.replies = [] + return r + + +def set_dronecan_param(name, value, uri='mcast:0', timeout=10.0, + save=True, restart=True, node_id=None): + '''set a DroneCAN parameter on the (single) AM32 node, optionally + save parameters and restart the node. Needs pydronecan''' + import dronecan + node = dronecan.make_node(uri, node_id=126, bitrate=1000000) + found = {} + + def on_status(e): + found[e.transfer.source_node_id] = True + + node.add_handler(dronecan.uavcan.protocol.NodeStatus, on_status) + # NodeStatus is 1Hz: collect for a couple of seconds so all bus + # participants are seen, not just the first + t0 = time.time() + while time.time() - t0 < timeout: + node.spin(0.1) + if found and time.time() - t0 > 2.5: + break + if not found: + raise RuntimeError('no DroneCAN node found') + + target = None + + def request_wait(req, timeout=2.0): + '''send a service request, spin until the response (or timeout + callback, which pydronecan delivers as None)''' + result = {} + + def cb(e): + result['response'] = e.response if e is not None else None + result['done'] = True + + node.request(req, target, cb) + deadline = time.time() + timeout + while 'done' not in result and time.time() < deadline: + node.spin(0.05) + return result.get('response') + + # other tools may be on the bus: find the AM32 node by probing for + # the parameter itself (a GetSet read of an unknown name returns an + # empty response name) + candidates = [node_id] if node_id is not None else sorted(found.keys()) + for cand in candidates: + target = cand + req = dronecan.uavcan.protocol.param.GetSet.Request() + req.name = name + rsp = request_wait(req, timeout=1.0) + if rsp is not None and len(rsp.name) > 0: + break + else: + raise RuntimeError('no node with parameter %s found' % name) + + req = dronecan.uavcan.protocol.param.GetSet.Request() + req.name = name + req.value = dronecan.uavcan.protocol.param.Value(integer_value=int(value)) + for attempt in range(3): + rsp = request_wait(req) + if rsp is not None and rsp.value.integer_value == int(value): + break + else: + raise RuntimeError('param set of %s failed' % name) + + if save: + req = dronecan.uavcan.protocol.param.ExecuteOpcode.Request() + req.opcode = req.OPCODE_SAVE + for attempt in range(3): + rsp = request_wait(req) + if rsp is not None and rsp.ok: + break + else: + raise RuntimeError('param save failed') + + if restart: + req = dronecan.uavcan.protocol.RestartNode.Request() + req.magic_number = req.MAGIC_NUMBER + request_wait(req, timeout=1.0) + + node.close() + return target diff --git a/Mcu/SITL/sitl_gui.py b/Mcu/SITL/sitl_gui.py new file mode 100644 index 000000000..c2d9f87b9 --- /dev/null +++ b/Mcu/SITL/sitl_gui.py @@ -0,0 +1,960 @@ +#!/usr/bin/env python3 +''' +control GUI for the AM32 SITL: drives PWM/DShot input over UDP and +DroneCAN input over mcast, with live telemetry from BDShot (including +extended DShot telemetry) and DroneCAN esc.Status. + +each input has an Enable checkbox that instantly starts/stops its stream, +for exercising failover between inputs. The parameter panel sets +INPUT_SIGNAL_TYPE etc over DroneCAN (needed because the DRONECAN_IN +default disables the PWM/DShot input interrupts). + +built on PySide6 (Qt): pyqtgraph plots and QGraphicsScene animation +panels can be added on this foundation. Install the dependencies with + python3 Mcu/SITL/make_gui_env.py + +usage: sitl_gui.py [--port 57733] [--can-uri mcast:0] + +with --control-port N the UI can additionally be driven by commands over +a localhost TCP connection (one per line), for scripted testing of the +actual UI paths: + ds_enable 0|1, ds_type pwm|dshot300|..., ds_bidir 0|1, ds_value N, + ds_rate N, ds_edt 0|1, zero, edt_enable, edt_disable, can_enable 0|1, + can_value X, can_rate N, param NAME VALUE, status, quit +responses go back to the client prefixed with OK/STATUS/ERR. A client +disconnect leaves the GUI running. +--log FILE records every UI action with a timestamp; --replay FILE plays +a recording back with its original timing. +''' + +import argparse +import os +import queue +import signal +import socket +import sys +import threading +import time + +try: + from PySide6.QtCore import Qt, QTimer, QRectF, QLineF + from PySide6.QtGui import QFontDatabase, QPen, QBrush, QColor, QPainter + from PySide6.QtWidgets import (QApplication, QCheckBox, QComboBox, + QGraphicsScene, QGraphicsView, + QGridLayout, QGroupBox, QHBoxLayout, + QDoubleSpinBox, QLabel, QPushButton, + QSlider, QSpinBox, QWidget) +except ImportError: + _here = os.path.dirname(os.path.abspath(__file__)) + if sys.platform == 'win32': + _venv_py = os.path.join(_here, 'venv', 'Scripts', 'python.exe') + else: + _venv_py = os.path.join(_here, 'venv', 'bin', 'python3') + _run_cmd = ' '.join([_venv_py, os.path.abspath(__file__)] + sys.argv[1:]) + if os.path.exists(_venv_py): + sys.stderr.write( + 'PySide6 is required for the SITL GUI. The GUI environment is\n' + 'already set up, run:\n' + ' %s\n' % _run_cmd) + else: + sys.stderr.write( + 'PySide6 is required for the SITL GUI. Either install the packages\n' + 'from %s into the system python,\n' + 'or create the self-contained environment with:\n' + ' python3 %s\n' + 'and then run:\n' + ' %s\n' + % (os.path.join(_here, 'requirements-gui.txt'), + os.path.join(_here, 'make_gui_env.py'), + _run_cmd)) + sys.exit(1) + +try: + import pyqtgraph as pg + HAVE_PYQTGRAPH = True +except ImportError: + HAVE_PYQTGRAPH = False + +import glob +import math + +import sitl_dshot as sd +from sitl_gui_backend import DshotPanel, CanPanel, SimStream, HAVE_DRONECAN + + +def main(): + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument('--host', default='127.0.0.1') + ap.add_argument('--port', type=int, default=57733) + ap.add_argument('--state-port', type=int, default=57734) + ap.add_argument('--can-uri', default='mcast:0') + ap.add_argument('--poles', type=int, default=14) + ap.add_argument('--control-port', type=int, default=0, + help='TCP port on localhost accepting UI control commands ' + '(for scripted tests, default off)') + ap.add_argument('--log', metavar='FILE', + help='log all UI actions with timestamps, for later --replay') + ap.add_argument('--replay', metavar='FILE', + help='replay a --log action file with its original timing') + args = ap.parse_args() + + t0 = time.time() + logf = open(args.log, 'w') if args.log else None + + def log_action(cmd): + if logf is not None: + logf.write('%.3f %s\n' % (time.time() - t0, cmd)) + logf.flush() + + ds = DshotPanel(args.host, args.port) + ds.poles = args.poles + sim = SimStream(args.host, args.state_port) + + # spawn the DroneCAN node with SIGINT ignored: its multiprocessing IO + # child inherits the kernel-level SIG_IGN disposition, so a terminal + # Ctrl-C only interrupts this process and the child is shut down + # through node.close() instead of dying with a traceback + signal.signal(signal.SIGINT, signal.SIG_IGN) + can = CanPanel(args.can_uri) if HAVE_DRONECAN else None + if can is not None: + can.started.wait(5.0) + + app = QApplication(sys.argv) + app.setStyle('Fusion') + win = QWidget() + win.setWindowTitle('AM32 SITL control') + top = QGridLayout(win) + + # ---- PWM/DShot input panel + f1 = QGroupBox('PWM/DShot input (udp %s:%u)' % (args.host, args.port)) + f1.setToolTip( + 'The classic ESC signal wire, emulated over UDP: each packet is one\n' + 'frame on the wire, either a servo PWM pulse or a complete DShot\n' + 'frame. The frames are decoded by the unmodified AM32 firmware,\n' + 'including input auto-detection, CRC checking and arming.') + g1 = QGridLayout(f1) + top.addWidget(f1, 0, 0) + + ds_enable = QCheckBox('Enable') + + def ds_enable_changed(): + ds.enabled = ds_enable.isChecked() + log_action('ds_enable %d' % int(ds.enabled)) + + ds_enable.setToolTip( + 'Start/stop the frame stream. The ESC arms after seeing zero\n' + 'throttle for about 1.5 seconds, like on the bench. Unchecking\n' + 'simulates signal loss: the firmware signal timeout zeroes the\n' + 'motor and reboots the ESC, which is the behaviour exercised in\n' + 'input failover testing.') + ds_enable.toggled.connect(ds_enable_changed) + g1.addWidget(ds_enable, 0, 0) + + g1.addWidget(QLabel('Type:'), 1, 0) + ds_type = QComboBox() + ds_type.addItems(sorted(sd.TYPE_NAMES.keys())) + ds_type.setCurrentText('dshot300') + + def type_changed(*a): + ds.ptype = sd.TYPE_NAMES[ds_type.currentText()] + is_pwm = ds.ptype == sd.TYPE_PWM + ds_value.setRange(1000 if is_pwm else 0, 2000 if is_pwm else 2047) + ds_value.setValue(1000 if is_pwm else 0) + if ds.ptype == sd.TYPE_DSHOT150: + # checkDshot() only has detection bands for dshot300/600 + ds.status = 'note: AM32 input detection does not support dshot150, use 300/600' + else: + ds.status = '' + log_action('ds_type %s' % ds_type.currentText()) + value_changed() + + ds_type.setToolTip( + 'Signal protocol on the wire.\n' + 'pwm: the classic servo pulse, 1000..2000us pulse width = throttle,\n' + ' sent at 50..490Hz.\n' + 'dshot300/600: digital frames at 300/600 kbit/s. Each frame is 16\n' + ' bits: 11 bit throttle (48..2047; 0=stop, 1..47 are commands),\n' + ' a telemetry request bit and a 4 bit checksum.\n' + 'dshot150 exists in the protocol but AM32 input auto-detection has\n' + 'no timing band for it, so the ESC never recognises it.') + ds_type.currentTextChanged.connect(type_changed) + g1.addWidget(ds_type, 1, 1) + + ds_bidir = QCheckBox('bidir (BDShot)') + + def ds_bidir_changed(): + ds.bidir = ds_bidir.isChecked() + log_action('ds_bidir %d' % int(ds.bidir)) + + ds_bidir.setToolTip( + 'Bidirectional DShot (BDShot): the signal line idles high and the\n' + 'frame checksum is inverted. After each received frame the ESC\n' + 'answers on the same wire with a GCR-encoded reply carrying the\n' + 'electrical rotation period, which flight controllers use for RPM\n' + 'notch filtering. The reply also carries extended telemetry frames\n' + 'when EDT is enabled. Required for the BDShot telemetry line below.') + ds_bidir.toggled.connect(ds_bidir_changed) + g1.addWidget(ds_bidir, 1, 2) + + g1.addWidget(QLabel('Throttle:'), 2, 0) + ds_value = QSlider(Qt.Horizontal) + ds_value.setRange(0, 2047) + ds_value.setMinimumWidth(260) + + def value_changed(*a): + v = ds_value.value() + log_action('ds_value %d' % v) + # never stream DShot values 1..47 from the throttle slider: they + # are commands (direction, bi_direction, save, programming mode) + # and a slider drag dwells long enough to execute them + if ds.ptype != sd.TYPE_PWM and 0 < v < 48: + v = 0 + ds.value = v + + ds_value.setToolTip( + 'Throttle value sent in every frame. DShot: 48..2047 (0 = motor\n' + 'stop). Values 1..47 are DShot commands (beacons, direction,\n' + 'settings save, programming mode) - the slider skips them because\n' + 'a drag would dwell long enough to execute one. PWM: pulse width\n' + 'in microseconds, 1000 = stop, 2000 = full throttle.') + # valueChanged fires for both drags and programmatic setValue() + ds_value.valueChanged.connect(value_changed) + g1.addWidget(ds_value, 2, 1, 1, 2) + ds_value_label = QLabel('0') + g1.addWidget(ds_value_label, 2, 3) + + g1.addWidget(QLabel('Rate Hz:'), 3, 0) + ds_rate = QSpinBox() + ds_rate.setRange(10, 4000) + ds_rate.setValue(500) + + def rate_changed(*a): + ds.rate = float(ds_rate.value()) + log_action('ds_rate %d' % ds_rate.value()) + + ds_rate.setToolTip( + 'Frame rate on the wire. Flight controllers send DShot at 1..8kHz\n' + 'and servo PWM at 50..490Hz. The ESC needs a continuous stream:\n' + 'arming, the signal-loss timeout and BDShot reply rate all follow\n' + 'the frame rate. Note the rate is wall clock, so at low simulation\n' + 'speedups frames arrive faster in simulated time and some are\n' + 'dropped while the virtual wire is busy, as on real hardware.') + ds_rate.valueChanged.connect(rate_changed) + g1.addWidget(ds_rate, 3, 1) + + bf = QHBoxLayout() + zero_btn = QPushButton('Zero throttle') + zero_btn.setToolTip( + 'Set the throttle to the stop value (DShot 0 / PWM 1000us).\n' + 'Needed for arming and before DShot commands such as the EDT\n' + 'enable are accepted by the firmware.') + zero_btn.clicked.connect( + lambda: ds_value.setValue(1000 if ds.ptype == sd.TYPE_PWM else 0)) + bf.addWidget(zero_btn) + + ds_edt = QCheckBox('EDT (extended telemetry)') + + def ds_edt_changed(): + ds.edt_want = ds_edt.isChecked() + log_action('ds_edt %d' % int(ds.edt_want)) + + ds_edt.setToolTip( + 'Extended DShot Telemetry: temperature, voltage and current frames\n' + 'interleaved into the BDShot replies (temperature/voltage every\n' + '~200 replies, current every ~40). The firmware only accepts the\n' + 'enable command (DShot command 13) while armed with the motor\n' + 'stopped, and a reboot clears it, so this checkbox keeps re-sending\n' + 'the command until the replies show EDT frames. Needs bidir.') + ds_edt.toggled.connect(ds_edt_changed) + bf.addWidget(ds_edt) + bf.addStretch(1) + g1.addLayout(bf, 4, 0, 1, 4) + + ds_status = QLabel('arm: enable + hold zero throttle >1.5s') + ds_status.setToolTip( + 'Guidance from the DShot sender: arming procedure, EDT progress\n' + 'and protocol notes.') + g1.addWidget(ds_status, 5, 0, 1, 4) + + # ---- DroneCAN input panel + f2 = QGroupBox('DroneCAN input (%s)' % args.can_uri) + f2.setToolTip( + 'DroneCAN: the CAN bus protocol used on larger vehicles, here\n' + 'carried over multicast UDP. Throttle goes as esc.RawCommand\n' + 'broadcasts and the ESC sends esc.Status telemetry back. In the\n' + 'current firmware, once any RawCommand has been received the CAN\n' + 'input overrides the PWM/DShot wire until a reboot - the\n' + 'arbitration behaviour the failover parameter work is about.') + g2 = QGridLayout(f2) + top.addWidget(f2, 0, 1) + + if can is not None: + can_enable = QCheckBox('Enable') + + def can_enable_changed(): + can.enabled = can_enable.isChecked() + log_action('can_enable %d' % int(can.enabled)) + + can_enable.setToolTip( + 'Master switch for this GUI\'s CAN traffic: ArmingStatus at the\n' + 'configured rate plus RawCommand when its box is ticked.\n' + 'Cutting everything simulates losing the flight controller\n' + 'entirely: the firmware zeroes throttle 250ms after commands\n' + 'stop and the CAN signal timeout then reboots the ESC. Use the\n' + 'RawCommand box instead to cut only the command stream.') + can_enable.toggled.connect(can_enable_changed) + g2.addWidget(can_enable, 0, 0) + + can_rawcmd = QCheckBox('RawCommand') + can_rawcmd.setChecked(True) + can_rawcmd.setToolTip( + 'Send the esc.RawCommand throttle broadcasts. Unchecking\n' + 'simulates a flight controller that stops commanding while the\n' + 'rest of its CAN traffic continues (the ArmingStatus stream\n' + 'keeps flowing) - the firmware is expected to zero the input\n' + '250ms after commands stop.') + + def can_rawcmd_changed(): + can.send_rawcommand = can_rawcmd.isChecked() + log_action('can_rawcmd %d' % int(can.send_rawcommand)) + + can_rawcmd.toggled.connect(can_rawcmd_changed) + g2.addWidget(can_rawcmd, 1, 0) + + can_armed = QCheckBox('Armed') + can_armed.setChecked(True) + can_armed.setToolTip( + 'Value carried in the ArmingStatus broadcasts: checked sends\n' + 'FULLY_ARMED (255), unchecked sends disarmed (0). With the\n' + 'default REQUIRE_ARM setting the ESC only applies throttle\n' + 'while armed, so unchecking this while spinning must stop the\n' + 'motor. The ArmingStatus stream itself follows Enable; note\n' + 'the ESC ignores NodeStatus, so ArmingStatus is what keeps its\n' + 'CAN signal timeout fed.') + + def can_armed_changed(): + can.armed = can_armed.isChecked() + log_action('can_armed %d' % int(can.armed)) + + can_armed.toggled.connect(can_armed_changed) + g2.addWidget(can_armed, 1, 1) + + g2.addWidget(QLabel('Throttle:'), 2, 0) + # slider in 0..1000 -> throttle 0..1 + can_value = QSlider(Qt.Horizontal) + can_value.setRange(0, 1000) + can_value.setMinimumWidth(200) + + def can_value_changed(*a): + can.throttle = can_value.value() / 1000.0 + log_action('can_value %.4f' % can.throttle) + + can_value.setToolTip( + 'Throttle 0..1, sent as esc.RawCommand 0..8191. The default\n' + 'firmware settings also require an ArmingStatus broadcast\n' + '(sent automatically while enabled) before spinning.') + can_value.valueChanged.connect(can_value_changed) + g2.addWidget(can_value, 2, 1, 1, 2) + can_value_label = QLabel('0.00') + g2.addWidget(can_value_label, 2, 3) + + g2.addWidget(QLabel('Rate Hz:'), 3, 0) + can_rate = QSpinBox() + can_rate.setRange(1, 1000) + can_rate.setValue(50) + + def can_rate_changed(*a): + can.rate = float(can_rate.value()) + log_action('can_rate %d' % can_rate.value()) + + can_rate.setToolTip( + 'RawCommand broadcast rate. Autopilots typically send ESC\n' + 'commands at 50..400Hz on CAN.') + can_rate.valueChanged.connect(can_rate_changed) + g2.addWidget(can_rate, 3, 1) + + can_zero_btn = QPushButton('Zero throttle') + can_zero_btn.setToolTip('Set the CAN throttle to zero (keeps streaming commands).') + can_zero_btn.clicked.connect(lambda: can_value.setValue(0)) + g2.addWidget(can_zero_btn, 4, 0) + + # parameter panel + pf = QGroupBox('parameters (set + save + restart)') + pf.setToolTip( + 'Sets an ESC setting over the DroneCAN parameter protocol, saves\n' + 'to eeprom and reboots the ESC so it takes effect at startup.\n' + 'INPUT_SIGNAL_TYPE selects which input the firmware listens to:\n' + 'the default 5 (dronecan) disables the PWM/DShot input\n' + 'interrupts entirely, so set 0..2 to test the signal wire.') + gp = QGridLayout(pf) + g2.addWidget(pf, 5, 0, 1, 4) + gp.addWidget(QLabel('INPUT_SIGNAL_TYPE:'), 0, 0) + ptype_var = QSpinBox() + ptype_var.setToolTip( + 'INPUT_SIGNAL_TYPE value: 0 = auto detect the wire protocol,\n' + '1 = dshot, 2 = servo PWM, 5 = dronecan only (disables the\n' + 'PWM/DShot input interrupts at boot).') + ptype_var.setRange(0, 5) + ptype_var.setValue(1) + gp.addWidget(ptype_var, 0, 1) + gp.addWidget(QLabel('(0=auto 1=dshot 2=servo 5=dronecan)'), 0, 2) + param_status = QLabel('') + gp.addWidget(param_status, 1, 0, 1, 3) + + def param_apply(): + log_action('param INPUT_SIGNAL_TYPE %d' % ptype_var.value()) + can.set_param('INPUT_SIGNAL_TYPE', ptype_var.value()) + + apply_btn = QPushButton('Apply') + apply_btn.setToolTip('Set the parameter over CAN, save to eeprom and reboot the ESC.') + apply_btn.clicked.connect(param_apply) + gp.addWidget(apply_btn, 0, 3) + else: + g2.addWidget(QLabel('pydronecan not available'), 0, 0) + + # ---- simulation panel: motor model selection and the optional high + # rate graph/animation views fed by the SITL state stream + f4 = QGroupBox('simulation') + f4.setToolTip( + 'Controls for the physics simulation itself (not the ESC firmware):\n' + 'the motor/battery model, the simulation pace and the high rate\n' + 'views fed by the simulation state stream.') + g4 = QGridLayout(f4) + top.addWidget(f4, 2, 0, 1, 2) + + models_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'models') + + g4.addWidget(QLabel('Motor model:'), 0, 0) + model_combo = QComboBox() + model_paths = {} + for path in sorted(glob.glob(os.path.join(models_dir, '*.json'))): + name = os.path.splitext(os.path.basename(path))[0] + model_paths[name] = path + model_combo.addItem(name) + model_combo.setToolTip( + 'Motor/battery models from Mcu/SITL/models/*.json: winding\n' + 'resistance and inductance, Kv (rpm per volt), pole count, rotor\n' + 'inertia and the propeller load (torque proportional to speed\n' + 'squared), plus battery voltage and internal resistance. These\n' + 'set how the virtual motor behaves; the ESC settings should\n' + 'match the motor, as on a real bench.') + g4.addWidget(model_combo, 0, 1) + + def model_load(): + name = model_combo.currentText() + if name in model_paths: + log_action('model %s' % name) + sim.load_model(model_paths[name]) + + load_btn = QPushButton('Load') + load_btn.setToolTip( + 'Apply the selected model to the running simulation. Switching\n' + 'while spinning is like swapping the motor mid-flight - expect\n' + 'desyncs; switch at zero throttle for clean results.') + load_btn.clicked.connect(model_load) + g4.addWidget(load_btn, 0, 2) + model_status = QLabel('') + g4.addWidget(model_status, 0, 3, 1, 2) + + graph_i_check = QCheckBox('Current graph') + graph_v_check = QCheckBox('Voltage graph') + motorview_check = QCheckBox('Motor view') + graph_i_check.setToolTip( + 'Open a scope window with the three phase currents and the battery\n' + 'current, sampled from the physics. In a 6-step drive two phases\n' + 'conduct at a time, so each phase carries a quasi-trapezoidal\n' + 'current envelope that steps at every commutation (six steps per\n' + 'electrical revolution).') + graph_v_check.setToolTip( + 'Open a scope window with the three phase terminal voltages and\n' + 'the battery voltage. At coarse sample periods it shows the duty\n' + 'proportional average; at fine periods (with a low speedup) the\n' + 'raw PWM switching, the back-EMF ramp on the floating phase (what\n' + 'the comparator senses for zero crossings) and the dead time diode\n' + 'spikes at each PWM edge become visible.') + motorview_check.setToolTip( + 'Open the animated motor/bridge view. Slow the simulation right\n' + 'down with the speedup slider to watch individual commutation\n' + 'steps rotate the stator field ahead of the rotor.') + g4.addWidget(graph_i_check, 1, 0) + g4.addWidget(graph_v_check, 1, 1) + g4.addWidget(motorview_check, 1, 2) + sim_rate_label = QLabel('') + sim_rate_label.setToolTip( + 'State stream rate actually arriving from the simulation (wall\n' + 'clock). It is the sample period in simulated time divided by the\n' + 'speedup, capped at about 200k samples/s.') + g4.addWidget(sim_rate_label, 1, 3, 1, 2) + + # simulation speedup, logarithmic 0.001x .. 2x, for slow motion in + # the motor view + g4.addWidget(QLabel('Speedup:'), 2, 0) + speed_slider = QSlider(Qt.Horizontal) + speed_slider.setRange(0, 165) + speed_slider.setValue(150) + speed_slider.setMinimumWidth(200) + speed_label = QLabel('1.000x') + + def slider_to_speedup(v): + return 10.0 ** ((v - 150) / 50.0) + + def speed_changed(*a): + speedup = slider_to_speedup(speed_slider.value()) + speed_label.setText('%.3fx' % speedup) + log_action('speedup %.4f' % speedup) + sim.set_speedup(speedup) + + speed_slider.setToolTip( + 'Simulation pace relative to wall clock, 0.001x to 2x. Everything\n' + 'inside the simulation (arming times, timeouts, telemetry rates)\n' + 'runs in simulated time, so at 0.01x the ESC responds 100x slower\n' + 'from the outside. Use slow motion to watch the motor view and to\n' + 'capture fine waveforms; the input frames you send arrive faster\n' + 'in simulated time, so some are dropped as on a busy wire.') + speed_slider.valueChanged.connect(speed_changed) + g4.addWidget(speed_slider, 2, 1, 1, 2) + g4.addWidget(speed_label, 2, 3) + speed_1x = QPushButton('1x') + speed_1x.setToolTip('Back to real time.') + speed_1x.clicked.connect(lambda: speed_slider.setValue(150)) + g4.addWidget(speed_1x, 2, 4) + + # scope controls: sample period and window, shared by both graph + # windows. Fine sample periods (down to the 500ns physics step, + # where the dead time windows are visible on the phase voltages) are + # meant to be used together with a low speedup to keep the wall + # clock data rate sane + sample_spin = QDoubleSpinBox() + sample_spin.setRange(0.5, 1000.0) + sample_spin.setValue(50.0) + sample_spin.setSuffix(' us sample') + sample_spin.setDecimals(1) + g4.addWidget(sample_spin, 3, 0, 1, 2) + window_spin = QDoubleSpinBox() + window_spin.setRange(0.05, 2000.0) + window_spin.setValue(50.0) + window_spin.setSuffix(' ms window') + window_spin.setDecimals(2) + g4.addWidget(window_spin, 3, 2) + + def sample_changed(*a): + sim.period_us = sample_spin.value() + log_action('sample_us %.1f' % sample_spin.value()) + + sample_spin.setToolTip( + 'Scope sample period in simulated microseconds. At 10us and above\n' + 'each sample is the average over its period (the honest duty\n' + 'proportional envelope - point sampling would alias against the\n' + 'PWM and show false gaps). Below 10us samples are instantaneous\n' + 'levels, down to the 500ns physics step, showing PWM edges and\n' + 'dead time. The wall clock rate is capped at ~200k samples/s, so\n' + 'fine periods only take full effect at low speedups.') + sample_spin.valueChanged.connect(sample_changed) + + def window_changed(*a): + log_action('window_ms %.2f' % window_spin.value()) + + window_spin.setToolTip( + 'Scope x axis span in simulated milliseconds, newest sample at the\n' + 'right edge. For commutation-scale viewing use 10..50ms; for PWM\n' + 'and dead time detail use 0.1..1ms together with a fine sample\n' + 'period and a low speedup.') + window_spin.valueChanged.connect(window_changed) + + # the scopes: each signal set gets its own top level pyqtgraph + # window, created lazily on first enable. Closing a window unchecks + # its box + SIGNAL_SETS = { + # key -> ((label, colour, sample column), ...), y axis label/unit + 'i': ((('iu', 'r', 4), ('iv', 'g', 5), ('iw', 'b', 6), + ('ibus', 'w', 11)), 'current', 'A'), + 'v': ((('vu', 'r', 7), ('vv', 'g', 8), ('vw', 'b', 9), + ('vbus', 'w', 10)), 'voltage', 'V'), + } + graph_windows = {} + + def update_sim_enable(): + sim.enabled = (graph_i_check.isChecked() or graph_v_check.isChecked() + or motorview_check.isChecked()) + + def graph_toggled(key, check, title): + log_action('graph_%s %d' % (key, int(check.isChecked()))) + if check.isChecked() and key not in graph_windows: + if not HAVE_PYQTGRAPH: + model_status.setText('pyqtgraph not available') + check.setChecked(False) + return + + class GraphWindow(pg.PlotWidget): + def closeEvent(self, ev): + check.setChecked(False) + ev.accept() + + w = GraphWindow() + w.setWindowTitle('AM32 SITL %s' % title) + if key == 'i': + w.setToolTip( + 'Phase currents iu/iv/iw (red/green/blue) and battery\n' + 'current ibus (white). Two phases conduct at a time in a\n' + '6-step drive: current flows in one phase, back through\n' + 'another, and the third floats near zero. Each commutation\n' + 'advances the pattern; torque is proportional to the\n' + 'conducted current. ibus is the PWM-chopped share drawn\n' + 'from the battery - averaged it equals duty times phase\n' + 'current, which is why battery current is far below phase\n' + 'current at partial throttle.') + else: + w.setToolTip( + 'Phase terminal voltages vu/vv/vw (red/green/blue) and\n' + 'battery voltage vbus (white). At coarse sample periods\n' + 'the trace is the duty proportional average. With a fine\n' + 'sample period and low speedup the raw switching shows:\n' + 'the PWM square wave on the driven phase, the low side\n' + 'held near 0V, the floating phase ramping with back-EMF\n' + 'through the zero crossing the comparator detects, and\n' + '500ns dead time spikes to -0.7V or vbus+0.7V where the\n' + 'body diodes conduct at each PWM edge.') + w.resize(700, 300) + w.addLegend(offset=(10, 10)) + w.setLabel('bottom', 'time', 's') + defs, label, unit = SIGNAL_SETS[key] + w.setLabel('left', label, unit) + curves = [(w.plot(pen=pg.mkPen(color, width=1), name=name), col) + for name, color, col in defs] + graph_windows[key] = (w, curves) + if key in graph_windows: + graph_windows[key][0].setVisible(check.isChecked()) + update_sim_enable() + + graph_i_check.toggled.connect( + lambda: graph_toggled('i', graph_i_check, 'phase currents')) + graph_v_check.toggled.connect( + lambda: graph_toggled('v', graph_v_check, 'phase voltages')) + + # motor/bridge animation, created lazily on first enable + view = None + scene = None + anim = {} + + def make_motor_view(): + nonlocal view, scene + scene = QGraphicsScene(0, 0, 420, 220) + view = QGraphicsView(scene) + view.setToolTip( + 'Live motor and bridge state.\n' + 'Dial: the orange needle is the mechanical rotor angle, the\n' + 'cyan needle the electrical angle - with 7 pole pairs the\n' + 'electrical needle turns 7x faster, one electrical turn per 6\n' + 'commutation steps.\n' + 'U/V/W bars: the bridge output for each phase - green = PWM\n' + 'driven high side, blue = tied low, grey = floating (undriven,\n' + 'used for back-EMF sensing), orange = brake PWM.\n' + 'The comparator line shows which floating phase is being\n' + 'watched for its back-EMF zero crossing, which is how a\n' + 'sensorless ESC knows the rotor position.\n' + 'Use the speedup slider for slow motion.') + view.setRenderHint(QPainter.Antialiasing) + view.setFixedHeight(240) + # rotor dial + scene.addEllipse(20, 20, 180, 180, QPen(QColor('gray'), 2)) + anim['needle'] = scene.addLine(QLineF(110, 110, 110, 30), QPen(QColor('orange'), 4)) + anim['e_needle'] = scene.addLine(QLineF(110, 110, 110, 60), QPen(QColor('cyan'), 2)) + scene.addSimpleText('rotor').setPos(95, 202) + # bridge legs: three vertical phase bars, coloured by mode + anim['legs'] = [] + for p, name in enumerate(('U', 'V', 'W')): + x = 240 + p * 55 + rect = scene.addRect(QRectF(x, 40, 36, 120), QPen(Qt.NoPen), QBrush(QColor('gray'))) + anim['legs'].append(rect) + label = scene.addSimpleText(name) + label.setPos(x + 12, 165) + anim['comp'] = scene.addSimpleText('') + anim['comp'].setPos(240, 190) + anim['rpm'] = scene.addSimpleText('') + anim['rpm'].setPos(240, 12) + top.addWidget(view, 4, 0, 1, 2) + + MODE_COLORS = { + 0: QColor(90, 90, 90), # FLOAT + 1: QColor(60, 100, 220), # LOW + 2: QColor(60, 190, 60), # PWM + 3: QColor(60, 190, 120), # PWM_NOCOMP + 4: QColor(220, 140, 40), # BRAKE_PWM + } + + def motorview_changed(): + log_action('motorview %d' % int(motorview_check.isChecked())) + if motorview_check.isChecked() and view is None: + make_motor_view() + if view is not None: + view.setVisible(motorview_check.isChecked()) + update_sim_enable() + win.adjustSize() + + motorview_check.toggled.connect(motorview_changed) + + def update_sim_views(): + visible = [gw for gw, _ in graph_windows.values() if gw.isVisible()] + if visible: + win_s = window_spin.value() * 1e-3 + w = sim.window(win_s) + if w: + t0 = w[-1][0] - win_s + ts = [smp[0] - t0 for smp in w] + for gw, curves in graph_windows.values(): + if gw.isVisible(): + for curve, col in curves: + curve.setData(ts, [smp[col] for smp in w]) + # the x axis is the window control, not auto range + gw.setXRange(0, win_s, padding=0) + if motorview_check.isChecked() and view is not None: + smp = sim.latest() + if smp is not None: + t, omega, theta, theta_e = smp[0], smp[1], smp[2], smp[3] + modes, comp_ph, comp_out = smp[12], smp[13], smp[14] + cx, cy, r = 110, 110, 80 + anim['needle'].setLine(QLineF(cx, cy, cx + r * math.sin(theta), + cy - r * math.cos(theta))) + re = r * 0.6 + anim['e_needle'].setLine(QLineF(cx, cy, cx + re * math.sin(theta_e), + cy - re * math.cos(theta_e))) + for p in range(3): + anim['legs'][p].setBrush(QBrush(MODE_COLORS.get(modes[p], QColor('gray')))) + anim['comp'].setText('comparator: phase %s out=%d' % ('UVW'[comp_ph], comp_out)) + anim['rpm'].setText('%.0f rpm' % (omega * 60 / (2 * math.pi))) + + sim_view_timer = QTimer() + sim_view_timer.timeout.connect(update_sim_views) + sim_view_timer.start(33) + + # ---- telemetry panel + f3 = QGroupBox('telemetry') + g3 = QGridLayout(f3) + top.addWidget(f3, 1, 0, 1, 2) + fixed = QFontDatabase.systemFont(QFontDatabase.FixedFont) + bds_label = QLabel('BDShot: -') + bds_label.setToolTip( + 'Telemetry decoded from the BDShot replies on the signal wire:\n' + 'rpm: from the eRPM period in each reply and the pole count.\n' + 'spinning/stopped: whether the replies report rotation.\n' + 'EDT on/off: whether extended telemetry frames are arriving.\n' + 'sent/replies: frame rates on the virtual wire; replies stop when\n' + ' the wire is saturated or the ESC is rebooting.\n' + 'badcrc: replies that failed their checksum.\n' + 'temp/volt/current: extended telemetry values when EDT is on.') + bds_label.setFont(fixed) + g3.addWidget(bds_label, 0, 0) + can_label = QLabel('DroneCAN: -') + can_label.setToolTip( + 'Telemetry from the DroneCAN esc.Status broadcasts:\n' + 'rpm/volt/cur/temp: as reported by the firmware (voltage and\n' + ' current from the simulated ADC path).\n' + 'err: desync count - commutation losses detected by the firmware.\n' + 'esc.Status: telemetry rate (TELEM_RATE parameter, sim time).\n' + 'cmds: RawCommand rate this GUI is sending.\n' + 'node: the ESC node id; up: firmware uptime in simulated seconds\n' + ' (resets on every reboot, so it exposes signal-timeout reboots).') + can_label.setFont(fixed) + g3.addWidget(can_label, 1, 0) + + # ---- optional TCP control interface, driving the same widgets and + # handlers as the mouse, so scripted tests cover the UI paths. + # Commands are one per line; OK/STATUS/ERR responses go back to the + # issuing client. A client disconnect leaves the GUI running; the + # quit command closes it + cmd_queue = queue.Queue() # (line, reply function) + + def emit(msg): + print(msg) + sys.stdout.flush() + + def control_client(conn): + def reply(msg): + try: + conn.sendall((msg + '\n').encode()) + except OSError: + pass + f = conn.makefile('r') + for line in f: + cmd_queue.put((line, reply)) + conn.close() + + def control_server(): + srv = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + srv.bind(('127.0.0.1', args.control_port)) + srv.listen(4) + while True: + conn, _ = srv.accept() + threading.Thread(target=control_client, args=(conn,), daemon=True).start() + + def handle_command(line, reply): + parts = line.split() + if not parts: + return + cmd, cargs = parts[0], parts[1:] + if cmd == 'ds_enable': + ds_enable.setChecked(bool(int(cargs[0]))) + elif cmd == 'ds_type': + ds_type.setCurrentText(cargs[0]) + elif cmd == 'ds_value': + ds_value.setValue(int(cargs[0])) + elif cmd == 'ds_bidir': + ds_bidir.setChecked(bool(int(cargs[0]))) + elif cmd == 'ds_rate': + ds_rate.setValue(int(cargs[0])) + elif cmd == 'zero': + ds_value.setValue(1000 if ds.ptype == sd.TYPE_PWM else 0) + elif cmd == 'ds_edt': + ds_edt.setChecked(bool(int(cargs[0]))) + elif cmd == 'edt_enable': + ds_edt.setChecked(True) + elif cmd == 'edt_disable': + ds_edt.setChecked(False) + elif cmd == 'can_enable' and can is not None: + can_enable.setChecked(bool(int(cargs[0]))) + elif cmd == 'can_rawcmd' and can is not None: + can_rawcmd.setChecked(bool(int(cargs[0]))) + elif cmd == 'can_armed' and can is not None: + can_armed.setChecked(bool(int(cargs[0]))) + elif cmd == 'can_value' and can is not None: + can_value.setValue(int(float(cargs[0]) * 1000)) + elif cmd == 'can_rate' and can is not None: + can_rate.setValue(int(cargs[0])) + elif cmd == 'param' and can is not None: + can.set_param(cargs[0], int(cargs[1])) + elif cmd == 'motorview': + motorview_check.setChecked(bool(int(cargs[0]))) + elif cmd == 'model': + if cargs[0] in model_paths: + model_combo.setCurrentText(cargs[0]) + model_load() + else: + sim.load_model(cargs[0]) + elif cmd == 'graph_i' or cmd == 'graphs': + graph_i_check.setChecked(bool(int(cargs[0]))) + elif cmd == 'graph_v': + graph_v_check.setChecked(bool(int(cargs[0]))) + elif cmd == 'signals': + # compatibility with recordings from the single-graph UI + if cargs[0] == 'voltages': + graph_v_check.setChecked(True) + else: + graph_i_check.setChecked(True) + elif cmd == 'sample_us': + sample_spin.setValue(float(cargs[0])) + elif cmd == 'window_ms': + window_spin.setValue(float(cargs[0])) + elif cmd == 'speedup': + x = float(cargs[0]) + pos = int(round(150 + 50 * math.log10(max(0.001, min(2.0, x))))) + if pos == speed_slider.value(): + speed_changed() + else: + speed_slider.setValue(pos) + elif cmd == 'status': + reply('STATUS %s' % bds_label.text()) + reply('STATUS %s' % can_label.text()) + reply('STATUS ds: %s' % (ds.status or '-')) + reply('STATUS sim: %s rate=%.0f/s' % (sim.model_status or '-', sim.rate.hz())) + return + elif cmd == 'quit': + app.quit() + return + else: + reply('ERR unknown command: %s' % line.strip()) + return + reply('OK %s' % line.strip()) + + def cmd_poll(): + while True: + try: + line, reply = cmd_queue.get_nowait() + except queue.Empty: + return + try: + handle_command(line, reply) + except Exception as ex: + reply('ERR %s: %s' % (line.strip(), ex)) + + def replay_reader(): + with open(args.replay) as f: + entries = [] + for line in f: + parts = line.strip().split(None, 1) + if len(parts) == 2: + entries.append((float(parts[0]), parts[1])) + rt0 = time.time() + for when, cmd in entries: + delay = rt0 + when - time.time() + if delay > 0: + time.sleep(delay) + cmd_queue.put((cmd + '\n', emit)) + emit('REPLAY done (%u actions)' % len(entries)) + + cmd_timer = QTimer() + cmd_timer.timeout.connect(cmd_poll) + if args.control_port > 0: + threading.Thread(target=control_server, daemon=True).start() + if args.replay: + threading.Thread(target=replay_reader, daemon=True).start() + if args.control_port > 0 or args.replay: + cmd_timer.start(50) + + def update(): + ds_value_label.setText(str(ds_value.value())) + ds_status.setText(ds.status or 'arm: enable + hold zero throttle >1.5s') + edt = ' '.join('%s=%s' % kv for kv in sorted(ds.edt_fresh().items())) + bds_label.setText('BDShot: rpm=%-6.0f %-8s EDT:%-3s sent=%.0f/s replies=%.0f/s badcrc=%u %s' + % (ds.rpm, 'spinning' if ds.spinning else 'stopped', + 'on' if ds.edt_active() else 'off', + ds.sent.hz(), ds.replies.hz(), ds.badcrc, edt)) + if can is not None: + can_value_label.setText('%.2f' % (can_value.value() / 1000.0)) + if can.error: + can_label.setText('DroneCAN: error: %s' % can.error) + else: + s = can.status + can_label.setText( + 'DroneCAN: rpm=%-6s volt=%-5s cur=%-5s temp=%-5s err=%-3s ' + 'esc.Status=%.0f/s cmds=%.0f/s node=%s up=%us' + % (s.get('rpm', '-'), + ('%.1f' % s['voltage']) if 'voltage' in s else '-', + ('%.1f' % s['current']) if 'current' in s else '-', + ('%.0f' % s['temp']) if 'temp' in s else '-', + s.get('errors', '-'), + can.esc_rate.hz(), can.sent.hz(), + can.node_id, can.uptime)) + try: + param_status.setText(can.param_result.get_nowait()) + except queue.Empty: + pass + model_status.setText(sim.model_status) + if sim.enabled: + sim_rate_label.setText('%.0f samples/s' % sim.rate.hz()) + else: + sim_rate_label.setText('') + + update_timer = QTimer() + update_timer.timeout.connect(update) + update_timer.start(100) + + # graceful Ctrl-C / SIGTERM: quit the event loop. The handler runs + # from the 100ms update timer, the next time python bytecode executes + signal.signal(signal.SIGINT, lambda *a: app.quit()) + signal.signal(signal.SIGTERM, lambda *a: app.quit()) + + win.show() + try: + app.exec() + finally: + ds.running = False + sim.close() + if can is not None: + can.running = False + # let the CAN thread close the node and its IO child + can.thread.join(2.0) + + +if __name__ == '__main__': + main() diff --git a/Mcu/SITL/sitl_gui_backend.py b/Mcu/SITL/sitl_gui_backend.py new file mode 100644 index 000000000..59d277dc7 --- /dev/null +++ b/Mcu/SITL/sitl_gui_backend.py @@ -0,0 +1,397 @@ +''' +UI-independent backends for the AM32 SITL control GUI: the PWM/DShot +sender, the DroneCAN node and rate counters. These run in their own +threads and expose plain attributes/queues, so they can be driven by any +front end or by headless tests without a display. +''' + +import collections +import queue +import socket +import struct +import threading +import time + +import sitl_dshot as sd + +try: + import dronecan + HAVE_DRONECAN = True +except ImportError: + HAVE_DRONECAN = False + + +class RateCounter(object): + def __init__(self): + self.count = 0 + self.rate = 0.0 + self.last = time.time() + self.last_count = 0 + + def tick(self, n=1): + self.count += n + + def hz(self): + now = time.time() + dt = now - self.last + if dt >= 1.0: + self.rate = (self.count - self.last_count) / dt + self.last = now + self.last_count = self.count + return self.rate + + +class DshotPanel(object): + '''PWM/DShot sender thread + state''' + + def __init__(self, host, port): + self.port = sd.InputPort(host, port) + self.enabled = False + self.ptype = sd.TYPE_DSHOT300 + self.bidir = False + self.telem_bit = False + self.value = 0 # dshot value or pwm width + self.rate = 500.0 + self.cmd_queue = queue.Queue() + self.sent = RateCounter() + self.replies = RateCounter() + self.rpm = 0.0 + self.spinning = False + self.badcrc = 0 + self.edt = {} # kind -> (value, time received) + self.edt_want = False + self.last_edt_seen = 0.0 + self.last_edt_cmd = 0.0 + self.poles = 14 + self.status = '' + self.running = True + threading.Thread(target=self._sender, daemon=True).start() + + def _sender(self): + next_send = time.time() + while self.running: + now = time.time() + if not self.enabled: + next_send = now + time.sleep(0.02) + self._collect() + continue + if now < next_send: + time.sleep(next_send - now) + now = time.time() + # catch-up burst: keep the average rate under coarse sleep + # granularity (VMs, CI runners) - the firmware's bidirectional + # auto-detect needs >100 frames before arming completes + burst = 0 + while now >= next_send and burst < 10: + next_send += 1.0 / max(1.0, self.rate) + try: + cmd = self.cmd_queue.get_nowait() + except queue.Empty: + cmd = None + if cmd is not None: + self.port.send_dshot(cmd, ptype=self.ptype, telem=True, bidir=self.bidir) + elif self.ptype == sd.TYPE_PWM: + self.port.send_pwm(int(self.value)) + else: + self.port.send_dshot(int(self.value), ptype=self.ptype, + telem=self.telem_bit, bidir=self.bidir) + self.sent.tick() + burst += 1 + if now - next_send > 0.25: + next_send = now # fell too far behind, resync + self._collect() + self._edt_maintain(now) + self.port.close() + + def _collect(self): + for r in self.port.get_replies(): + self.replies.tick() + kind, val = sd.decode_reply(r[3], edt_expected=True) + if kind == 'erpm': + self.spinning = val < 65408 + self.rpm = sd.erpm_period_to_rpm(val, self.poles) + elif kind == 'badcrc': + self.badcrc += 1 + else: + self.last_edt_seen = time.time() + if kind == 'edt' and val in (0xE00, 0xEFF): + # EDT init/deinit acknowledgement frames, not data + continue + self.edt[kind] = (val, time.time()) + + def edt_active(self): + '''true when EDT frames are actually arriving from the ESC''' + return time.time() - self.last_edt_seen < 3.0 + + def edt_fresh(self, max_age=15.0): + '''EDT values received recently. The age allows for the slow EDT + schedule at low frame rates (temp/voltage every ~400 replies)''' + if not self.edt_active(): + return {} + now = time.time() + return {k: v for k, (v, t) in self.edt.items() if now - t < max_age} + + def send_command(self, cmd, count=8): + for _ in range(count): + self.cmd_queue.put(cmd) + + def _edt_maintain(self, now): + '''EDT is a maintained state: the firmware only processes DShot + commands while armed with the motor stopped, silently discarding + them otherwise, and a reboot clears EDT. Keep (re)sending the + enable/disable command until the reply stream matches the + requested state''' + if self.ptype == sd.TYPE_PWM or not self.bidir: + return + active = self.edt_active() + if self.edt_want == active: + if self.edt_want and self.status.startswith('EDT'): + self.status = '' + return + if int(self.value) != 0 or self.spinning: + if self.edt_want: + self.status = 'EDT pending: needs the motor stopped at zero throttle' + return + if now - self.last_edt_cmd > 1.5: + self.last_edt_cmd = now + if self.edt_want: + self.send_command(sd.DSHOT_CMD_EDT_ENABLE) + self.status = 'EDT enable sent, waiting for EDT frames (arms after >1.5s at zero)' + else: + self.send_command(sd.DSHOT_CMD_EDT_DISABLE) + self.status = 'EDT disable sent' + + +class CanPanel(object): + '''DroneCAN node thread: RawCommand/ArmingStatus stream, telemetry + handlers and parameter set requests''' + + def __init__(self, uri): + self.uri = uri + self.enabled = False + self.send_rawcommand = True + self.armed = True + self.throttle = 0.0 # 0..1 + self.rate = 50.0 + self.esc_index = 0 + self.status = {} + self.node_id = None + self.uptime = 0 + self.esc_rate = RateCounter() + self.sent = RateCounter() + self.param_queue = queue.Queue() + self.param_result = queue.Queue() + self.error = None + self.running = True + # set once make_node has spawned the IO child (or failed), so the + # caller can sequence signal handler setup around the spawn + self.started = threading.Event() + self.thread = threading.Thread(target=self._can_thread, daemon=True) + self.thread.start() + + def _can_thread(self): + try: + node = dronecan.make_node(self.uri, node_id=126, bitrate=1000000) + except Exception as ex: + self.error = str(ex) + self.started.set() + return + self.started.set() + + def on_esc_status(e): + m = e.message + if m.esc_index != self.esc_index: + return + # the ESC is unambiguously the sender of esc.Status; other + # nodes on the bus also send NodeStatus + self.node_id = e.transfer.source_node_id + self.esc_rate.tick() + self.status = { + 'rpm': m.rpm, + 'voltage': m.voltage, + 'current': m.current, + 'temp': m.temperature - 273.15, + 'errors': m.error_count, + } + + def on_node_status(e): + if e.transfer.source_node_id == self.node_id: + self.uptime = e.message.uptime_sec + + node.add_handler(dronecan.uavcan.equipment.esc.Status, on_esc_status) + node.add_handler(dronecan.uavcan.protocol.NodeStatus, on_node_status) + + next_send = time.time() + while self.running: + try: + node.spin(0.002) + except Exception: + pass + self._handle_param(node) + if not self.enabled: + next_send = time.time() + continue + now = time.time() + if now >= next_send: + next_send += 1.0 / max(1.0, self.rate) + status = 255 if self.armed else 0 + node.broadcast(dronecan.uavcan.equipment.safety.ArmingStatus(status=status)) + if self.send_rawcommand: + cmds = [0] * (self.esc_index + 1) + cmds[self.esc_index] = int(8191 * self.throttle) + node.broadcast(dronecan.uavcan.equipment.esc.RawCommand(cmd=cmds)) + self.sent.tick() + # orderly shutdown of the mcast IO child process + try: + node.close() + except Exception: + pass + + def _handle_param(self, node): + try: + name, value = self.param_queue.get_nowait() + except queue.Empty: + return + if self.node_id is None: + self.param_result.put('no node seen yet') + return + target = self.node_id + result = {} + + def cb(e): + result['rsp'] = e.response if e is not None else None + result['done'] = True + + def wait(req, timeout=2.0): + result.clear() + node.request(req, target, cb) + deadline = time.time() + timeout + while 'done' not in result and time.time() < deadline: + node.spin(0.05) + return result.get('rsp') + + req = dronecan.uavcan.protocol.param.GetSet.Request() + req.name = name + req.value = dronecan.uavcan.protocol.param.Value(integer_value=int(value)) + rsp = wait(req) + if rsp is None or len(rsp.name) == 0: + self.param_result.put('%s: set failed' % name) + return + req = dronecan.uavcan.protocol.param.ExecuteOpcode.Request() + req.opcode = req.OPCODE_SAVE + wait(req) + req = dronecan.uavcan.protocol.RestartNode.Request() + req.magic_number = req.MAGIC_NUMBER + wait(req, timeout=1.0) + self.param_result.put('%s=%d saved, node %d restarted' % (name, int(value), target)) + + def set_param(self, name, value): + self.param_queue.put((name, value)) + + +class SimStream(object): + """subscriber for the SITL simulation state stream (--state-port): + high rate physics samples for graphs/animation, and runtime motor + model loading. Samples are (t_s, omega, theta, theta_e, iu, iv, iw, + vu, vv, vw, vbus, ibus, modes, comp_phase, comp_out)""" + + SAMPLE = struct.Struct('= 10 else 0 + pkt = struct.pack(' len(d): + break + smp = self.SAMPLE.unpack_from(d, off) + batch.append((smp[0] * 1e-9,) + smp[1:]) + with self.lock: + self.samples.extend(batch) + self.rate.tick(len(batch)) + + def latest(self): + with self.lock: + return self.samples[-1] if self.samples else None + + def window(self, seconds): + """most recent samples spanning the given time window""" + out = [] + with self.lock: + if not self.samples: + return out + t_end = self.samples[-1][0] + for smp in reversed(self.samples): + if t_end - smp[0] > seconds: + break + out.append(smp) + out.reverse() + return out + + def set_speedup(self, speedup): + pkt = struct.pack('= nxt and burst < 10: + nxt += 1.0 / self.rate + if self.cmds: + self.port.send_dshot(self.cmds.pop(0), ptype=self.ptype, + telem=True, bidir=self.bidir) + elif self.ptype == sd.TYPE_PWM: + self.port.send_pwm(int(self.value)) + else: + self.port.send_dshot(int(self.value), ptype=self.ptype, + bidir=self.bidir) + burst += 1 + if now - nxt > 0.25: + nxt = now + time.sleep(0.0005) + + def stop(self): + self.running = False + self.port.close() + + +def rpm_from_state(sim, window=1.0): + w = sim.window(window) + if not w: + return -1.0 + return sum(s[1] for s in w) / len(w) * 60.0 / 6.28318 + + +def wait_for_state(sim, timeout=5.0): + deadline = time.time() + timeout + while time.time() < deadline: + if sim.samples: + return True + time.sleep(0.1) + return bool(sim.samples) + + +def open_state(host, port, period_us=200): + sim = SimStream(host, port, period_us=period_us) + sim.enabled = True + return sim diff --git a/Mcu/SITL/tests/conftest.py b/Mcu/SITL/tests/conftest.py new file mode 100644 index 000000000..114cb2611 --- /dev/null +++ b/Mcu/SITL/tests/conftest.py @@ -0,0 +1,110 @@ +'''pytest fixtures for AM32 SITL CI tests.''' + +from __future__ import annotations + +import os +import sys +import tempfile + +import pytest + +HERE = os.path.dirname(os.path.abspath(__file__)) +SITL_DIR = os.path.normpath(os.path.join(HERE, '..')) +sys.path.insert(0, SITL_DIR) + +from sitl_harness import ( # noqa: E402 + Sitl, + SitlStartError, + find_sitl_binary, + free_mcast_group, + open_state, +) + + +def pytest_addoption(parser): + parser.addoption( + '--sitl', + action='store', + default=None, + help='path to AM32 SITL binary (default: newest obj/AM32_AM32_SITL_CAN_*.elf)', + ) + + +@pytest.fixture(scope='session') +def sitl_path(request): + path = find_sitl_binary(request.config.getoption('--sitl')) + if not path or not os.path.exists(path): + pytest.fail( + 'SITL binary not found. Build with: make AM32_SITL_CAN\n' + 'or pass --sitl /path/to/elf') + return path + + +@pytest.fixture +def workdir(tmp_path): + '''isolated cwd so eeprom files and logs do not collide''' + d = tempfile.mkdtemp(prefix='sitl_ci_', dir=str(tmp_path)) + cwd = os.getcwd() + os.chdir(d) + try: + yield d + finally: + os.chdir(cwd) + + +@pytest.fixture +def sitl_factory(sitl_path, workdir): + '''factory: sitl_factory(extra_args=..., can_uri=..., **kw) -> Sitl''' + instances = [] + + def _make(extra_args=(), can_uri='none', **kw): + s = Sitl(sitl_path, extra_args=extra_args, workdir=workdir, + can_uri=can_uri, **kw) + instances.append(s) + return s + + yield _make + for s in instances: + s.close() + + +@pytest.fixture +def mcast_uri(): + return 'mcast:%d' % free_mcast_group() + + +@pytest.fixture +def sitl_can_factory(sitl_factory): + '''like sitl_factory, but skip the test if multicast CAN cannot start. + + GitHub macOS runners historically lack a usable multicast route; the + SITL either dies during CAN init or never becomes reachable. Match + upstream behaviour: skip rather than fail the job. + ''' + + def _make(extra_args=(), can_uri=None, **kw): + if can_uri is None: + raise TypeError('sitl_can_factory requires can_uri') + try: + return sitl_factory(extra_args=extra_args, can_uri=can_uri, **kw) + except SitlStartError as e: + if e.looks_like_mcast_failure: + pytest.skip('SITL multicast CAN unavailable on this host:\n%s' + % e) + raise + + return _make + + +@pytest.fixture +def state_stream(): + streams = [] + + def _open(sitl, period_us=200): + sim = open_state('127.0.0.1', sitl.state_port, period_us=period_us) + streams.append(sim) + return sim + + yield _open + for s in streams: + s.close() diff --git a/Mcu/SITL/tests/test_boot.py b/Mcu/SITL/tests/test_boot.py new file mode 100644 index 000000000..48ba0485f --- /dev/null +++ b/Mcu/SITL/tests/test_boot.py @@ -0,0 +1,41 @@ +'''SITL process boot and basic I/O surface.''' + +from __future__ import annotations + +import os +import time + +import pytest + +from sitl_harness import wait_for_state + + +def test_boot_binds_ports_and_streams_state(sitl_factory, state_stream): + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim, timeout=5.0), ( + 'state stream never started\n' + sitl.log_tail()) + latest = sim.latest() + assert latest is not None + # omega ~ 0 at idle, bus voltage from default model (~16.8 V) + omega, vbus = latest[1], latest[10] + assert abs(omega) < 50.0, 'motor spinning at idle: omega=%g' % omega + assert 10.0 < vbus < 30.0, 'unexpected bus voltage: %g' % vbus + + +def test_boot_verbose_log_mentions_ports(sitl_factory): + sitl = sitl_factory(extra_args=['--input-type', '1', '--verbose'], + can_uri='none', wait_s=1.0) + # give the verbose 1 Hz line a chance; port banners print at startup + time.sleep(0.5) + log = sitl.log_tail(40) + assert 'PWM/DShot input on udp port' in log or str(sitl.input_port) in log, log + assert 'state/model port' in log or str(sitl.state_port) in log, log + + +def test_eeprom_file_created(sitl_factory, workdir): + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none', wait_s=0.8) + eeprom = os.path.join(workdir, 'am32_eeprom.bin') + assert os.path.exists(eeprom), 'eeprom not created in %s: %s' % ( + workdir, os.listdir(workdir)) + assert os.path.getsize(eeprom) > 0 diff --git a/Mcu/SITL/tests/test_dronecan.py b/Mcu/SITL/tests/test_dronecan.py new file mode 100644 index 000000000..ef3d3e090 --- /dev/null +++ b/Mcu/SITL/tests/test_dronecan.py @@ -0,0 +1,98 @@ +'''DroneCAN RawCommand / telemetry path over multicast UDP.''' + +from __future__ import annotations + +import time + +import pytest + +from sitl_harness import rpm_from_state, wait_for_state + +dronecan = pytest.importorskip('dronecan') + + +def _multicast_usable(sitl, sim, timeout=5.0): + '''GitHub macOS (and some locked-down VMs) have no multicast route. + Skip rather than fail when the SITL never starts streaming with CAN on.''' + if wait_for_state(sim, timeout=timeout): + return True + pytest.skip( + 'SITL state stream never started with CAN enabled; ' + 'multicast is probably unavailable.\n' + sitl.log_tail()) + + +def test_dronecan_throttle_and_esc_status(sitl_can_factory, state_stream, mcast_uri): + sitl = sitl_can_factory( + extra_args=['--node-id', '10'], + can_uri=mcast_uri, + wait_s=1.0) + sim = state_stream(sitl) + _multicast_usable(sitl, sim) + + node = dronecan.make_node(mcast_uri, node_id=100, bitrate=1000000) + status = {} + + def on_esc(e): + status['rpm'] = e.message.rpm + status['voltage'] = e.message.voltage + status['count'] = status.get('count', 0) + 1 + + node.add_handler(dronecan.uavcan.equipment.esc.Status, on_esc) + try: + t0 = time.time() + nxt = t0 + while time.time() - t0 < 10: + node.spin(0) + now = time.time() + if now >= nxt: + nxt += 0.02 + thr = 0.35 if now - t0 > 2.5 else 0.0 + node.broadcast( + dronecan.uavcan.equipment.safety.ArmingStatus(status=255)) + node.broadcast( + dronecan.uavcan.equipment.esc.RawCommand( + cmd=[int(8191 * thr)])) + time.sleep(0.001) + + rpm = rpm_from_state(sim) + assert 3500 <= rpm <= 6500, 'state rpm=%.0f' % rpm + assert 3500 <= status.get('rpm', -1) <= 6500, status + assert 15 < status.get('voltage', 0) < 18, status + assert status.get('count', 0) >= 3, 'too few esc.Status: %s' % status + finally: + # stop motor + for _ in range(10): + node.broadcast(dronecan.uavcan.equipment.esc.RawCommand(cmd=[0])) + node.spin(0.02) + node.close() + + +def test_dronecan_requires_arming(sitl_can_factory, state_stream, mcast_uri): + '''default REQUIRE_ARMING=1: RawCommand alone must not spin the motor''' + sitl = sitl_can_factory( + extra_args=['--node-id', '10'], + can_uri=mcast_uri, + wait_s=1.0) + sim = state_stream(sitl) + _multicast_usable(sitl, sim) + + node = dronecan.make_node(mcast_uri, node_id=101, bitrate=1000000) + try: + t0 = time.time() + nxt = t0 + while time.time() - t0 < 5.0: + node.spin(0) + now = time.time() + if now >= nxt: + nxt += 0.02 + # no ArmingStatus — only RawCommand at mid throttle + node.broadcast( + dronecan.uavcan.equipment.esc.RawCommand(cmd=[4000])) + time.sleep(0.001) + rpm = rpm_from_state(sim, 0.5) + assert rpm < 500, 'spun without arming: rpm=%.0f' % rpm + finally: + for _ in range(5): + node.broadcast(dronecan.uavcan.equipment.esc.RawCommand(cmd=[0])) + node.spin(0.02) + node.close() diff --git a/Mcu/SITL/tests/test_dshot.py b/Mcu/SITL/tests/test_dshot.py new file mode 100644 index 000000000..daf18d73b --- /dev/null +++ b/Mcu/SITL/tests/test_dshot.py @@ -0,0 +1,97 @@ +'''PWM/DShot input path tests against the SITL motor model.''' + +from __future__ import annotations + +import time + +import pytest + +import sitl_dshot as sd +from sitl_harness import Sender, rpm_from_state, wait_for_state + + +def _run_throttle(sitl, state_stream, ptype, value, rpm_lo, rpm_hi, + bidir=False, edt=False, arm_s=2.2, run_s=4.0, stop_s=3.0): + sim = state_stream(sitl) + assert wait_for_state(sim), 'state stream dead\n' + sitl.log_tail() + tx = Sender('127.0.0.1', sitl.input_port, ptype, bidir=bidir) + try: + time.sleep(arm_s) + if edt: + tx.cmds = [sd.DSHOT_CMD_EDT_ENABLE] * 8 + time.sleep(0.5) + tx.value = value + time.sleep(run_s) + rpm = rpm_from_state(sim) + assert rpm_lo <= rpm <= rpm_hi, ( + 'rpm=%.0f expected %d..%d\n%s' % (rpm, rpm_lo, rpm_hi, sitl.log_tail())) + + if bidir: + replies = tx.port.reply_count + assert replies > 500, 'too few BDShot replies: %d' % replies + erpm = [sd.decode_reply(r[3], edt_expected=edt) + for r in tx.port.get_replies()] + rpms = [sd.erpm_period_to_rpm(v) for k, v in erpm if k == 'erpm'] + if rpms: + assert abs(rpms[-1] - rpm) < max(200, rpm * 0.05), ( + 'bdshot=%.0f state=%.0f' % (rpms[-1], rpm)) + if edt: + edt_vals = {k: v for k, v in erpm + if k in ('temp', 'volt', 'current')} + assert edt_vals.get('temp') == 25, edt_vals + assert 14 < edt_vals.get('volt', 0) < 18, edt_vals + + # must stop again at zero throttle + tx.value = 0 + time.sleep(stop_s) + rpm = rpm_from_state(sim, 0.3) + assert rpm < 500, 'motor did not stop: rpm=%.0f' % rpm + finally: + tx.stop() + + +def test_dshot600_bidir_edt(sitl_factory, state_stream): + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + _run_throttle(sitl, state_stream, sd.TYPE_DSHOT600, value=800, + rpm_lo=4000, rpm_hi=7000, bidir=True, edt=True) + + +def test_dshot300(sitl_factory, state_stream): + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + _run_throttle(sitl, state_stream, sd.TYPE_DSHOT300, value=600, + rpm_lo=3000, rpm_hi=6000) + + +def test_zero_throttle_stays_stopped(sitl_factory, state_stream): + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim) + tx = Sender('127.0.0.1', sitl.input_port, sd.TYPE_DSHOT600, bidir=True) + try: + tx.value = 0 + time.sleep(3.0) + rpm = rpm_from_state(sim, 0.5) + assert rpm < 200, 'spun at zero throttle: rpm=%.0f' % rpm + finally: + tx.stop() + + +def test_bad_crc_frames_do_not_arm_or_spin(sitl_factory, state_stream): + '''all-corrupted frames must never produce throttle''' + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim) + port = sd.InputPort('127.0.0.1', sitl.input_port) + try: + t0 = time.time() + nxt = t0 + while time.time() - t0 < 4.0: + now = time.time() + while now >= nxt: + nxt += 1.0 / 500.0 + port.send_dshot(800, ptype=sd.TYPE_DSHOT600, corrupt=True) + time.sleep(0.0005) + rpm = rpm_from_state(sim, 0.5) + assert rpm < 200, 'motor spun on bad-CRC frames: rpm=%.0f' % rpm + finally: + port.close() diff --git a/Mcu/SITL/tests/test_models.py b/Mcu/SITL/tests/test_models.py new file mode 100644 index 000000000..1da77969e --- /dev/null +++ b/Mcu/SITL/tests/test_models.py @@ -0,0 +1,65 @@ +'''Runtime motor model load over the SITL state port.''' + +from __future__ import annotations + +import os +import time + +import pytest + +from sitl_harness import SITL_DIR, rpm_from_state, wait_for_state +import sitl_dshot as sd +from sitl_harness import Sender + + +MODELS = os.path.join(SITL_DIR, 'models') + + +@pytest.mark.parametrize('model', [ + 'racer_5inch.json', + 'default_7inch.json', + 'heavy_13inch.json', + 'unloaded.json', +]) +def test_load_stock_model(sitl_factory, state_stream, model): + path = os.path.join(MODELS, model) + assert os.path.isfile(path), path + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim) + sim.load_model(path) + # model_status is updated asynchronously over UDP + deadline = time.time() + 3.0 + status = '' + while time.time() < deadline: + status = sim.model_status or '' + if status and 'fail' not in status.lower() and 'error' not in status.lower(): + if 'loading' not in status.lower() or 'ok' in status.lower() or model in status: + break + time.sleep(0.1) + # Accept either an explicit OK-style status or a cleared error-free load + assert 'error' not in status.lower() and 'fail' not in status.lower(), status + + +def test_racer_model_spins_under_dshot(sitl_factory, state_stream): + '''heavier/lighter models still produce plausible RPM under DShot''' + path = os.path.join(MODELS, 'racer_5inch.json') + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim) + sim.load_model(path) + time.sleep(0.5) + + tx = Sender('127.0.0.1', sitl.input_port, sd.TYPE_DSHOT600) + try: + time.sleep(2.2) + tx.value = 700 + time.sleep(4.0) + rpm = rpm_from_state(sim) + # racer is higher kV / lighter load — allow a wide but non-zero band + assert 2000 <= rpm <= 15000, 'rpm=%.0f out of range on racer model' % rpm + tx.value = 0 + time.sleep(3.0) + assert rpm_from_state(sim, 0.3) < 800 + finally: + tx.stop() diff --git a/Mcu/SITL/tests/test_params.py b/Mcu/SITL/tests/test_params.py new file mode 100644 index 000000000..65e8413c5 --- /dev/null +++ b/Mcu/SITL/tests/test_params.py @@ -0,0 +1,140 @@ +'''DroneCAN parameter GetSet / save path.''' + +from __future__ import annotations + +import time + +import pytest + +from sitl_harness import wait_for_state + +dronecan = pytest.importorskip('dronecan') + + +def _wait_for_node(uri, timeout=8.0, our_id=110): + node = dronecan.make_node(uri, node_id=our_id, bitrate=1000000) + found = {} + + def on_status(e): + found[e.transfer.source_node_id] = True + + node.add_handler(dronecan.uavcan.protocol.NodeStatus, on_status) + deadline = time.time() + timeout + while time.time() < deadline and not found: + node.spin(0.1) + return node, found + + +def _request_wait(node, target, req, timeout=2.5): + '''send a service request; pydronecan delivers timeout as e=None''' + result = {} + + def cb(e): + result['response'] = e.response if e is not None else None + result['done'] = True + + node.request(req, target, cb) + deadline = time.time() + timeout + while 'done' not in result and time.time() < deadline: + node.spin(0.05) + return result.get('response') + + +def _get_param(node, target, name, attempts=5): + '''GetSet with retries — idle SITL reboots every ~2s on signal timeout, + which can swallow a single in-flight service transfer.''' + for _ in range(attempts): + req = dronecan.uavcan.protocol.param.GetSet.Request() + req.name = name + rsp = _request_wait(node, target, req) + if rsp is not None and str(rsp.name): + return rsp + time.sleep(0.3) + return None + + +def _set_param(node, target, name, value, attempts=5): + for _ in range(attempts): + req = dronecan.uavcan.protocol.param.GetSet.Request() + req.name = name + req.value = dronecan.uavcan.protocol.param.Value(integer_value=int(value)) + rsp = _request_wait(node, target, req) + if rsp is not None and str(rsp.name): + if int(rsp.value.integer_value) == int(value): + return rsp + time.sleep(0.3) + return None + + +def _require_mcast_node(sitl, sim, mcast_uri, our_id=110): + if not wait_for_state(sim, timeout=5.0): + pytest.skip('multicast/state unavailable\n' + sitl.log_tail()) + node, found = _wait_for_node(mcast_uri, our_id=our_id) + if 10 not in found: + node.close() + pytest.skip('ESC node 10 not seen on %s (mcast likely broken)\n%s' + % (mcast_uri, sitl.log_tail())) + return node, found + + +def test_param_get_defaults(sitl_can_factory, state_stream, mcast_uri): + sitl = sitl_can_factory( + extra_args=['--node-id', '10'], + can_uri=mcast_uri, + wait_s=1.0) + sim = state_stream(sitl) + node, found = _require_mcast_node(sitl, sim, mcast_uri) + try: + rsp = _get_param(node, 10, 'MOTOR_POLES') + assert rsp is not None, 'GetSet failed for MOTOR_POLES\n' + sitl.log_tail() + assert int(rsp.value.integer_value) == 14, rsp.value + rsp = _get_param(node, 10, 'INPUT_SIGNAL_TYPE') + assert rsp is not None + assert int(rsp.value.integer_value) in (0, 1, 2, 5), rsp.value + rsp = _get_param(node, 10, 'TELEM_RATE') + assert rsp is not None + assert 0 <= int(rsp.value.integer_value) <= 200 + finally: + node.close() + + +def test_param_set_telem_rate(sitl_can_factory, state_stream, mcast_uri): + sitl = sitl_can_factory( + extra_args=['--node-id', '10'], + can_uri=mcast_uri, + wait_s=1.0) + sim = state_stream(sitl) + node, found = _require_mcast_node(sitl, sim, mcast_uri, our_id=111) + try: + new_rate = 50 + rsp = _set_param(node, 10, 'TELEM_RATE', new_rate) + assert rsp is not None, 'set TELEM_RATE failed\n' + sitl.log_tail() + assert int(rsp.value.integer_value) == new_rate, rsp.value + rsp = _get_param(node, 10, 'TELEM_RATE') + assert rsp is not None + assert int(rsp.value.integer_value) == new_rate + finally: + node.close() + + +def test_param_save_opcode(sitl_can_factory, state_stream, mcast_uri): + sitl = sitl_can_factory( + extra_args=['--node-id', '10'], + can_uri=mcast_uri, + wait_s=1.0) + sim = state_stream(sitl) + node, found = _require_mcast_node(sitl, sim, mcast_uri, our_id=112) + try: + assert _set_param(node, 10, 'BEEP_VOLUME', 7) is not None + ok = False + for _ in range(5): + req = dronecan.uavcan.protocol.param.ExecuteOpcode.Request() + req.opcode = req.OPCODE_SAVE + rsp = _request_wait(node, 10, req, timeout=3.0) + if rsp is not None and rsp.ok: + ok = True + break + time.sleep(0.3) + assert ok, 'OPCODE_SAVE failed\n' + sitl.log_tail() + finally: + node.close() diff --git a/Mcu/SITL/tests/test_pwm.py b/Mcu/SITL/tests/test_pwm.py new file mode 100644 index 000000000..eae569987 --- /dev/null +++ b/Mcu/SITL/tests/test_pwm.py @@ -0,0 +1,29 @@ +'''Servo / PWM input path.''' + +from __future__ import annotations + +import time + +import sitl_dshot as sd +from sitl_harness import Sender, rpm_from_state, wait_for_state + + +def test_pwm_midstick_spins_and_stops(sitl_factory, state_stream): + sitl = sitl_factory(extra_args=['--input-type', '2'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim), sitl.log_tail() + tx = Sender('127.0.0.1', sitl.input_port, sd.TYPE_PWM) + try: + # arm at 1000 us + tx.value = 1000 + time.sleep(2.2) + tx.value = 1500 + time.sleep(4.0) + rpm = rpm_from_state(sim) + assert 4000 <= rpm <= 9000, 'rpm=%.0f expected 4000..9000' % rpm + tx.value = 1000 + time.sleep(3.0) + rpm = rpm_from_state(sim, 0.3) + assert rpm < 500, 'did not stop: rpm=%.0f' % rpm + finally: + tx.stop() diff --git a/Mcu/SITL/tests/test_safety.py b/Mcu/SITL/tests/test_safety.py new file mode 100644 index 000000000..54f436106 --- /dev/null +++ b/Mcu/SITL/tests/test_safety.py @@ -0,0 +1,258 @@ +'''Safety / arming functional tests. + +Firmware contract (Src/main.c, signal.c, DroneCAN.c): + - DShot/PWM: arming needs ~1s of valid signal with zero throttle + (adjusted_input == 0 and zero_input_count > 30). High throttle from + boot must never spin the motor. + - DroneCAN defaults REQUIRE_ARMING=1 and REQUIRE_ZERO_THROTTLE=1: + high RawCommand without ArmingStatus must not spin; high throttle + at first contact even with ArmingStatus must not spin until a zero + throttle period has armed the ESC. + - Bad / missing signal must not leave the motor running. +''' + +from __future__ import annotations + +import time + +import pytest + +import sitl_dshot as sd +from sitl_harness import Sender, rpm_from_state, wait_for_state + + +def _need_dronecan(): + return pytest.importorskip('dronecan') + + +def _assert_stopped(sim, label, window=0.5, limit=200.0): + rpm = rpm_from_state(sim, window) + assert rpm < limit, '%s: motor spinning rpm=%.0f (limit %.0f)' % ( + label, rpm, limit) + + +def _assert_spinning(sim, label, lo=2000.0, hi=20000.0, window=1.0): + rpm = rpm_from_state(sim, window) + assert lo <= rpm <= hi, '%s: rpm=%.0f expected %.0f..%.0f' % ( + label, rpm, lo, hi) + + +# --------------------------------------------------------------------------- +# DShot / PWM: boot with high throttle must not spin +# --------------------------------------------------------------------------- + +def test_dshot_high_throttle_from_boot_does_not_spin(sitl_factory, state_stream): + '''never send zero — high DShot from the first frame must not arm.''' + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim) + tx = Sender('127.0.0.1', sitl.input_port, sd.TYPE_DSHOT600) + try: + tx.value = 1000 # mid-high throttle immediately, no zero period + time.sleep(4.0) + _assert_stopped(sim, 'dshot high-from-boot', window=0.8) + finally: + tx.stop() + + +def test_pwm_high_throttle_from_boot_does_not_spin(sitl_factory, state_stream): + '''high PWM pulse from boot (no 1000 us arming) must not spin.''' + sitl = sitl_factory(extra_args=['--input-type', '2'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim) + tx = Sender('127.0.0.1', sitl.input_port, sd.TYPE_PWM) + try: + tx.value = 1800 # high stick from first frame + time.sleep(4.0) + _assert_stopped(sim, 'pwm high-from-boot', window=0.8) + finally: + tx.stop() + + +def test_dshot_arms_only_after_zero_then_spins(sitl_factory, state_stream): + '''high-from-boot blocked, then zero arms, then throttle spins, then stop.''' + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim) + tx = Sender('127.0.0.1', sitl.input_port, sd.TYPE_DSHOT600) + try: + tx.value = 900 + time.sleep(2.5) + _assert_stopped(sim, 'pre-arm high') + + tx.value = 0 + time.sleep(2.2) + _assert_stopped(sim, 'arming zero') + + tx.value = 700 + time.sleep(3.5) + _assert_spinning(sim, 'after proper arm', lo=3000, hi=9000) + + tx.value = 0 + time.sleep(3.0) + _assert_stopped(sim, 'post-run zero', window=0.4, limit=500) + finally: + tx.stop() + + +def test_pwm_arms_only_after_low_then_spins(sitl_factory, state_stream): + sitl = sitl_factory(extra_args=['--input-type', '2'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim) + tx = Sender('127.0.0.1', sitl.input_port, sd.TYPE_PWM) + try: + tx.value = 1600 + time.sleep(2.5) + _assert_stopped(sim, 'pre-arm high pwm') + + tx.value = 1000 + time.sleep(2.2) + _assert_stopped(sim, 'arming low pwm') + + tx.value = 1500 + time.sleep(3.5) + _assert_spinning(sim, 'after pwm arm', lo=3500, hi=10000) + + tx.value = 1000 + time.sleep(3.0) + _assert_stopped(sim, 'post-run low pwm', window=0.4, limit=500) + finally: + tx.stop() + + +# --------------------------------------------------------------------------- +# DroneCAN arming / zero-throttle safety +# --------------------------------------------------------------------------- + +def _can_drive(dronecan, node, thr, armed=True, rate_hz=50.0, duration=3.0, + send_arming_status=True): + '''broadcast ArmingStatus + RawCommand for duration seconds. + + armed=True → FULLY_ARMED (255) + armed=False → SAFE (0) so REQUIRE_ARMING zeros the applied throttle; + merely stopping ArmingStatus traffic leaves the last + armed state latched in the ESC. + ''' + t0 = time.time() + nxt = t0 + period = 1.0 / rate_hz + arm_status = 255 if armed else 0 + while time.time() - t0 < duration: + node.spin(0) + now = time.time() + if now >= nxt: + nxt += period + if send_arming_status: + node.broadcast( + dronecan.uavcan.equipment.safety.ArmingStatus( + status=arm_status)) + node.broadcast( + dronecan.uavcan.equipment.esc.RawCommand( + cmd=[int(8191 * thr)])) + time.sleep(0.001) + + +def test_dronecan_high_throttle_from_boot_does_not_spin( + sitl_can_factory, state_stream, mcast_uri): + '''REQUIRE_ZERO_THROTTLE=1: arm + high RawCommand from first contact + must not spin until a zero-throttle arming window has passed.''' + dronecan = _need_dronecan() + sitl = sitl_can_factory( + extra_args=['--node-id', '10'], can_uri=mcast_uri, wait_s=1.0) + sim = state_stream(sitl) + if not wait_for_state(sim, timeout=5.0): + pytest.skip('multicast unavailable\n' + sitl.log_tail()) + + node = dronecan.make_node(mcast_uri, node_id=120, bitrate=1000000) + try: + # no zero period — arm + 50% throttle immediately + _can_drive(dronecan, node, thr=0.5, armed=True, duration=4.0) + _assert_stopped(sim, 'can high-from-boot', window=0.8, limit=500) + finally: + _can_drive(dronecan, node, thr=0.0, armed=True, duration=0.5) + node.close() + + +def test_dronecan_arms_after_zero_then_spins( + sitl_can_factory, state_stream, mcast_uri): + dronecan = _need_dronecan() + sitl = sitl_can_factory( + extra_args=['--node-id', '10'], can_uri=mcast_uri, wait_s=1.0) + sim = state_stream(sitl) + if not wait_for_state(sim, timeout=5.0): + pytest.skip('multicast unavailable\n' + sitl.log_tail()) + + node = dronecan.make_node(mcast_uri, node_id=121, bitrate=1000000) + try: + _can_drive(dronecan, node, thr=0.5, armed=True, duration=2.5) + _assert_stopped(sim, 'can pre-arm high', limit=500) + + _can_drive(dronecan, node, thr=0.0, armed=True, duration=2.5) + _assert_stopped(sim, 'can arming zero', limit=500) + + _can_drive(dronecan, node, thr=0.35, armed=True, duration=4.0) + _assert_spinning(sim, 'can after proper arm', lo=3000, hi=8000) + + # Prop inertia coasts for a while after input goes to zero; match + # the DShot post-run settle (3s) so CI hosts with variable load + # are not flaky around ~1k rpm residual. + _can_drive(dronecan, node, thr=0.0, armed=True, duration=3.0) + _assert_stopped(sim, 'can post-run zero', window=0.5, limit=800) + finally: + _can_drive(dronecan, node, thr=0.0, armed=False, duration=0.3) + node.close() + + +def test_dronecan_disarm_zeros_input( + sitl_can_factory, state_stream, mcast_uri): + '''dropping ArmingStatus while running must stop applying throttle + (REQUIRE_ARMING=1 forces input to 0 when disarmed).''' + dronecan = _need_dronecan() + sitl = sitl_can_factory( + extra_args=['--node-id', '10'], can_uri=mcast_uri, wait_s=1.0) + sim = state_stream(sitl) + if not wait_for_state(sim, timeout=5.0): + pytest.skip('multicast unavailable\n' + sitl.log_tail()) + + node = dronecan.make_node(mcast_uri, node_id=122, bitrate=1000000) + try: + _can_drive(dronecan, node, thr=0.0, armed=True, duration=2.5) + _can_drive(dronecan, node, thr=0.35, armed=True, duration=4.0) + _assert_spinning(sim, 'can spinning before disarm', lo=3000, hi=8000) + + # keep high RawCommand but publish SAFE ArmingStatus + _can_drive(dronecan, node, thr=0.35, armed=False, duration=3.0) + _assert_stopped(sim, 'can after SAFE arming status', window=0.6, limit=800) + finally: + _can_drive(dronecan, node, thr=0.0, armed=False, duration=0.3) + node.close() + + +# --------------------------------------------------------------------------- +# Signal loss / stop behaviour (DShot) +# --------------------------------------------------------------------------- + +def test_dshot_signal_loss_stops_motor(sitl_factory, state_stream): + '''after arming and spinning, ceasing frames must stop the motor + (signal timeout → allOff / reset). Allow a reboot; omega must fall.''' + sitl = sitl_factory(extra_args=['--input-type', '1'], can_uri='none') + sim = state_stream(sitl) + assert wait_for_state(sim) + tx = Sender('127.0.0.1', sitl.input_port, sd.TYPE_DSHOT600) + try: + time.sleep(2.2) + tx.value = 700 + time.sleep(3.5) + _assert_spinning(sim, 'before signal loss', lo=3000, hi=9000) + + tx.stop() # stop sending entirely + # armed timeout ~0.5s, then freewheel coast + possible reboot (~2s). + # Prop inertia can leave >1–2k rpm after only 3s wall time on a + # loaded CI host — wait for a full coast-down before asserting. + time.sleep(5.0) + _assert_stopped(sim, 'after signal loss', window=0.5, limit=800) + finally: + try: + tx.stop() + except Exception: + pass diff --git a/Src/DroneCAN/DroneCAN.c b/Src/DroneCAN/DroneCAN.c index 900712c45..215575d36 100644 --- a/Src/DroneCAN/DroneCAN.c +++ b/Src/DroneCAN/DroneCAN.c @@ -10,10 +10,10 @@ #include "peripherals.h" #include "serial_telemetry.h" -#include -#include -#include -#include +#include "common.h" +#include "signal.h" +#include "version.h" +#include "eeprom.h" #include #include #include @@ -60,7 +60,7 @@ const struct { uint32_t crc2; // crc32 from end of app_signature to end of fw char mcu[16]; uint32_t unused[2]; -} app_signature __attribute__((section(".app_signature"))) = { +} app_signature AM32_FLASH_SECTION(".app_signature") = { .magic1 = APP_SIGNATURE_MAGIC1, .magic2 = APP_SIGNATURE_MAGIC2, .fwlen = 0, @@ -256,6 +256,11 @@ static uint64_t micros64(void) { static uint64_t base_us; static uint16_t last_cnt; + // the static state must be updated atomically, this is called from + // both interrupt handlers and the main loop. Save and restore + // PRIMASK so a caller's critical section is not ended early + const uint32_t primask = __get_PRIMASK(); + __disable_irq(); #ifdef ARTERY uint16_t cnt = UTILITY_TIMER->cval; #else @@ -265,7 +270,11 @@ static uint64_t micros64(void) base_us += 0x10000; } last_cnt = cnt; - return base_us + cnt; + const uint64_t ret = base_us + cnt; + if (!primask) { + __enable_irq(); + } + return ret; } /* @@ -286,6 +295,16 @@ static const uint8_t default_settings[] = { 0x80, 0x80, 0x80, 0x32, 0x00, 0x32, 0x00, 0x00, 0x0f, 0x0a, 0x0a, 0x8d, 0x66, 0x06, 0x01, 0x00 }; +#ifdef MCU_SITL +// let the SITL eeprom emulation seed a missing eeprom file with defaults +const uint8_t* DroneCAN_default_settings(unsigned* len); +const uint8_t* DroneCAN_default_settings(unsigned* len) +{ + *len = sizeof(default_settings); + return default_settings; +} +#endif + static const uint8_t advance_level_v3_remap[] = { 0x00, 0x08, 0x10, 0x16 // old values 0-3 map to new values 0,8,16,22 }; @@ -427,7 +446,7 @@ static void handle_param_GetSet(CanardInstance* ins, CanardRxTransfer* transfer) } if ((uint8_t *)p->ptr == &eepromBuffer.advance_level) { // automatically remap old values - if (pkt.value.integer_value < sizeof(advance_level_v3_remap)) { + if ((uint64_t)pkt.value.integer_value < sizeof(advance_level_v3_remap)) { pkt.value.integer_value = advance_level_v3_remap[pkt.value.integer_value]; } // adjust for advance level offset for eeprom v3 @@ -1187,6 +1206,9 @@ static void DroneCAN_Startup(void) NVIC_DisableIRQ(DMA1_Channel6_IRQn); NVIC_DisableIRQ(EXINT15_10_IRQn); EXINT->inten &= ~EXINT_LINE_15; +#elif defined(MCU_SITL) + NVIC_DisableIRQ(SITL_IRQ_DMA); + NVIC_DisableIRQ(SITL_IRQ_EXTI15); #else #error "unsupported MCU" #endif @@ -1248,8 +1270,14 @@ void DroneCAN_update() canstats.last_raw_command_us = 0; set_input(0); } - if (ts - canstats.last_raw_command_us > TARGET_PERIOD_US) { - // ensure at least 1kHz signal is seen by main code + if (canstats.last_raw_command_us != 0 && ts - canstats.last_raw_command_us > TARGET_PERIOD_US) { + /* + ensure at least 1kHz signal is seen by main code. Only once we + have received a RawCommand: set_input() overrides the + dshot/servo input state, so injecting it before CAN is + actually the input source would kill PWM/DShot input on any + node with a CAN node ID + */ set_input(last_can_input); canstats.last_raw_command_us = ts; } diff --git a/Src/DroneCAN/filter.c b/Src/DroneCAN/filter.c index 668d28d09..6ebdb97cc 100644 --- a/Src/DroneCAN/filter.c +++ b/Src/DroneCAN/filter.c @@ -63,6 +63,7 @@ float Filter2P_apply(const float sample, float cutoff_freq, float sample_freq) return output; } +#ifndef MCU_SITL /* unfortunately the maths libraries have an abort() linkage */ @@ -70,3 +71,4 @@ void abort(void) { __builtin_unreachable(); } +#endif diff --git a/Src/DroneCAN/sys_can_SITL.c b/Src/DroneCAN/sys_can_SITL.c new file mode 100644 index 000000000..21ca2f54a --- /dev/null +++ b/Src/DroneCAN/sys_can_SITL.c @@ -0,0 +1,428 @@ +/* + sys_can_SITL.c - CAN over multicast UDP for the SITL build. + + Wire compatible with the DroneCAN "mcast:N" URI as implemented in + libcanard drivers/mcast/mcast.c and ArduPilot SITL: group 239.65.82.N + port 57732, packets carrying a 10 byte header (magic, crc16-CCITT, + flags, 29 bit message id) followed by the frame data, with the DLC + implied by the datagram length. An optional interface may be given as + "mcast:N:lo" or "mcast:N:192.168.1.5". + */ + +#include "targets.h" + +#if DRONECAN_SUPPORT && defined(MCU_SITL) + +#include "sys_can.h" +#include "sitl.h" +#include "sitl_config.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define MCAST_ADDRESS_BASE "239.65.82.0" +#define MCAST_PORT 57732 +#define MCAST_MAGIC 0x2934 +#define MCAST_FLAG_CANFD 0x0001 + +struct __attribute__((packed)) mcast_pkt { + uint16_t magic; + uint16_t crc; + uint16_t flags; + uint32_t message_id; + uint8_t data[CANARD_CAN_FRAME_MAX_DATA_LEN]; +}; +#define MCAST_HDR_LEN 10 + +static int fd_in = -1; +static int fd_out = -1; + +static uint16_t crc16_CCITT(const uint8_t* buf, uint32_t len) +{ + uint16_t crc = 0xFFFF; + while (len--) { + crc ^= (uint16_t)(*buf++) << 8; + for (int i = 0; i < 8; i++) { + if (crc & 0x8000) { + crc = (crc << 1) ^ 0x1021; + } else { + crc <<= 1; + } + } + } + return crc; +} + +void sys_can_init(void) +{ + const char* name = sitl_cfg.can_uri; + fprintf(stderr, "SITL: CAN init %s\n", name); + if (strcmp(name, "none") == 0) { + // no CAN bus: the node stays in DNA allocation forever, so + // DroneCAN never injects throttle. Used for pure PWM/DShot tests + fprintf(stderr, "SITL: CAN disabled\n"); + return; + } + int bus_num = 0; + const char* ifname = NULL; + if (strncmp(name, "mcast:", 6) == 0 && name[6] != 0) { + bus_num = atoi(name + 6); + const char* colon = strchr(name + 6, ':'); + if (colon != NULL && colon[1] != 0) { + ifname = colon + 1; + } + } + if (bus_num < 0 || bus_num > 9) { + fprintf(stderr, "SITL: invalid mcast bus %d\n", bus_num); + exit(1); + } + + // optional interface, by name or IPv4 address + struct in_addr if_addr; + bool have_if = false; + if (ifname != NULL) { + if (inet_pton(AF_INET, ifname, &if_addr) == 1) { + have_if = true; + } else { + struct ifreq ifr; + memset(&ifr, 0, sizeof(ifr)); + strncpy(ifr.ifr_name, ifname, IFNAMSIZ - 1); + const int s = socket(AF_INET, SOCK_DGRAM, 0); + if (s < 0 || ioctl(s, SIOCGIFADDR, &ifr) != 0) { + fprintf(stderr, "SITL: no IPv4 address for interface %s\n", ifname); + exit(1); + } + if_addr = ((struct sockaddr_in*)&ifr.ifr_addr)->sin_addr; + close(s); + have_if = true; + } + } + char address[32]; + strncpy(address, MCAST_ADDRESS_BASE, sizeof(address) - 1); + address[sizeof(address) - 1] = 0; + address[strlen(address) - 1] = '0' + bus_num; + + struct sockaddr_in addr; + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(MCAST_PORT); + inet_pton(AF_INET, address, &addr.sin_addr); + + fd_in = sitl_udp_socket(); + if (fd_in < 0) { + perror("SITL: can socket"); + exit(1); + } + const int one = 1; + setsockopt(fd_in, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); +#ifdef SO_REUSEPORT + // macOS needs SO_REUSEPORT for multiple SITL instances / pydronecan + // on the same mcast group+port + setsockopt(fd_in, SOL_SOCKET, SO_REUSEPORT, &one, sizeof(one)); +#endif + struct sockaddr_in bind_addr = addr; +#if defined(__CYGWIN__) || defined(_WIN32) || defined(__APPLE__) + // Windows and macOS cannot reliably bind to a multicast group + // address; bind the port on INADDR_ANY and rely on the group + // membership plus the packet magic/CRC for filtering, as + // pydronecan's mcast driver does + bind_addr.sin_addr.s_addr = htonl(INADDR_ANY); +#endif + if (bind(fd_in, (struct sockaddr*)&bind_addr, sizeof(bind_addr)) != 0) { + perror("SITL: can bind"); + exit(1); + } + struct ip_mreq mreq; + memset(&mreq, 0, sizeof(mreq)); + mreq.imr_multiaddr = addr.sin_addr; + mreq.imr_interface.s_addr = have_if ? if_addr.s_addr : htonl(INADDR_ANY); + if (setsockopt(fd_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) != 0) { + perror("SITL: can multicast join"); + exit(1); + } + + fd_out = sitl_udp_socket(); + if (fd_out < 0) { + perror("SITL: can tx socket"); + exit(1); + } + if (have_if) { + struct sockaddr_in src; + memset(&src, 0, sizeof(src)); + src.sin_family = AF_INET; + src.sin_addr = if_addr; + if (bind(fd_out, (struct sockaddr*)&src, sizeof(src)) != 0 || + setsockopt(fd_out, IPPROTO_IP, IP_MULTICAST_IF, &if_addr, sizeof(if_addr)) != 0) { + perror("SITL: can tx interface"); + exit(1); + } + } + if (connect(fd_out, (struct sockaddr*)&addr, sizeof(addr)) != 0) { + perror("SITL: can tx socket"); + exit(1); + } + +#if defined(__CYGWIN__) || defined(_WIN32) + /* + no TX self test on Windows: a socket never receives its own + multicast there, so the test always fails, and the 127.0.0.1 + rebind is wrong (the loopback interface has no multicast). On a + multi homed machine pass an explicit interface: mcast:0: + */ +#else + /* + self test: verify our transmissions are delivered back to us. With + a "ip route add 239.65.82.0/24 dev lo" style route the kernel picks + a non-loopback source address and then drops the packet on receive, + breaking multicast silently. Rebinding the sender to 127.0.0.1 + fixes that case. With an explicit interface in the URI we respect + it and only warn + */ + for (int attempt = have_if ? 1 : 0; attempt < 2; attempt++) { + uint8_t probe[4] = { 0xde, 0xad, 0xbe, 0xef }; // wrong magic, ignored by receivers + // Prefer sendto over a connected send so a dead mcast route does + // not depend on SIGPIPE being ignored (belt-and-braces with + // signal(SIGPIPE, SIG_IGN) / SO_NOSIGPIPE). +#if defined(MSG_NOSIGNAL) + sendto(fd_out, probe, sizeof(probe), MSG_NOSIGNAL, + (struct sockaddr*)&addr, sizeof(addr)); +#else + sendto(fd_out, probe, sizeof(probe), 0, + (struct sockaddr*)&addr, sizeof(addr)); +#endif + struct pollfd pfd = { .fd = fd_in, .events = POLLIN, .revents = 0 }; + bool got = false; + while (poll(&pfd, 1, 50) == 1) { + uint8_t buf[MCAST_HDR_LEN + 64]; + const ssize_t ret = recv(fd_in, buf, sizeof(buf), MSG_DONTWAIT); + if (ret == (ssize_t)sizeof(probe) && memcmp(buf, probe, sizeof(probe)) == 0) { + got = true; + break; + } + } + if (got) { + break; + } + if (attempt == 0) { + // retry with the sender bound to loopback + close(fd_out); + fd_out = sitl_udp_socket(); + struct sockaddr_in lo_addr; + memset(&lo_addr, 0, sizeof(lo_addr)); + lo_addr.sin_family = AF_INET; + inet_pton(AF_INET, "127.0.0.1", &lo_addr.sin_addr); + if (fd_out < 0 || bind(fd_out, (struct sockaddr*)&lo_addr, sizeof(lo_addr)) != 0 || connect(fd_out, (struct sockaddr*)&addr, sizeof(addr)) != 0) { + perror("SITL: can tx socket (loopback)"); + exit(1); + } + fprintf(stderr, "SITL: CAN multicast loopback failed, using 127.0.0.1 source\n"); + } else { + fprintf(stderr, + "SITL: WARNING: CAN multicast self test failed, check the route for " + "%s (a 'dev lo' route needs 'src 127.0.0.1')\n", + address); + } + } +#endif + + fprintf(stderr, "SITL: CAN on %s (%s:%d)\n", name, address, MCAST_PORT); +} + +int16_t sys_can_transmit(const CanardCANFrame* txf) +{ + if (fd_out < 0) { + return -1; + } + struct mcast_pkt pkt; + pkt.magic = MCAST_MAGIC; + pkt.flags = 0; + pkt.message_id = txf->id; + memcpy(pkt.data, txf->data, txf->data_len); + pkt.crc = crc16_CCITT((const uint8_t*)&pkt.flags, txf->data_len + 6); +#if defined(MSG_NOSIGNAL) + const ssize_t ret = send(fd_out, &pkt, txf->data_len + MCAST_HDR_LEN, MSG_NOSIGNAL); +#else + const ssize_t ret = send(fd_out, &pkt, txf->data_len + MCAST_HDR_LEN, 0); +#endif + if (ret < 0) { + return (errno == EAGAIN || errno == EWOULDBLOCK) ? 0 : -1; + } + canstats.num_tx_interrupts++; + return 1; +} + +int16_t sys_can_receive(CanardCANFrame* rx_frame) +{ + if (fd_in < 0) { + return -1; + } + struct mcast_pkt pkt; + const ssize_t ret = recv(fd_in, &pkt, sizeof(pkt), MSG_DONTWAIT); + if (ret < 0) { + return (errno == EAGAIN || errno == EWOULDBLOCK) ? 0 : -1; + } + if (ret < MCAST_HDR_LEN || pkt.magic != MCAST_MAGIC) { + canstats.rxframe_error++; + return 0; + } + if (pkt.crc != crc16_CCITT((const uint8_t*)&pkt.flags, ret - 4)) { + canstats.rxframe_error++; + return 0; + } + rx_frame->id = pkt.message_id; + rx_frame->data_len = ret - MCAST_HDR_LEN; + memcpy(rx_frame->data, pkt.data, rx_frame->data_len); + return 1; +} + +/* + called from the SITL sim thread every 100us: if frames are waiting, + deliver them through the interrupt mechanism so handling happens with + the firmware thread suspended, like a real CAN RX interrupt + */ +void sitl_can_poll(void); +void sitl_can_poll(void) +{ + if (fd_in < 0) { + return; + } + struct pollfd pfd = { .fd = fd_in, .events = POLLIN, .revents = 0 }; + if (poll(&pfd, 1, 0) == 1) { + sitl_irq_pend(SITL_IRQ_CAN); + } +} + +/* + CAN RX "interrupt handler", called by sitl_it.c in interrupt context + */ +void sitl_can_irq(void); +void sitl_can_irq(void) +{ + CanardCANFrame rx_frame; + while (sys_can_receive(&rx_frame) == 1) { + canstats.num_rx_interrupts++; + DroneCAN_handleFrame(&rx_frame); + } +} + +// CAN statistics for the SITL --verbose output +void sitl_can_stats(uint32_t stats[4]); +void sitl_can_stats(uint32_t stats[4]) +{ + stats[0] = canstats.num_rx_interrupts; + stats[1] = canstats.num_commands; + stats[2] = canstats.rxframe_error; + stats[3] = (uint32_t)canstats.rx_ecode; +} + +void sys_can_disable_IRQ(void) +{ + sitl_nvic_disable_irq(SITL_IRQ_CAN); +} + +void sys_can_enable_IRQ(void) +{ + sitl_nvic_enable_irq(SITL_IRQ_CAN); +} + +/* + a stable 16 byte unique ID, from --uid if given, otherwise derived from + hostname and eeprom path so DNA node IDs stick across restarts + */ +void sys_can_getUniqueID(uint8_t id[16]) +{ + char seed[256]; + if (sitl_cfg.uid != NULL) { + strncpy(seed, sitl_cfg.uid, sizeof(seed) - 1); + seed[sizeof(seed) - 1] = 0; + } else { + char host[128] = "sitl"; + gethostname(host, sizeof(host) - 1); + snprintf(seed, sizeof(seed), "%s:%s", host, sitl_cfg.eeprom_path); + } + // FNV-1a expanded over 4 lanes + for (int lane = 0; lane < 4; lane++) { + uint32_t h = 2166136261U + lane * 16777619U; + for (const char* p = seed; *p; p++) { + h = (h ^ (uint8_t)*p) * 16777619U; + } + memcpy(&id[lane * 4], &h, 4); + } +} + +/* + RTC backup survives hardware reset. SITL reboots via execv, so keep the + eight words next to the eeprom file (`.rtc`) for warm-boot / + FW-update handoff (RTC_BKUP0_BOOTED / SIGNAL / FWUPDATE). + */ +static uint32_t rtc_backup[8]; +static bool rtc_loaded; + +static void rtc_backup_path(char* path, size_t pathlen) +{ + snprintf(path, pathlen, "%s.rtc", sitl_cfg.eeprom_path); +} + +static void rtc_backup_load(void) +{ + if (rtc_loaded) { + return; + } + rtc_loaded = true; + char path[512]; + rtc_backup_path(path, sizeof(path)); + FILE* f = fopen(path, "rb"); + if (!f) { + return; + } + if (fread(rtc_backup, 1, sizeof(rtc_backup), f) != sizeof(rtc_backup)) { + memset(rtc_backup, 0, sizeof(rtc_backup)); + } + fclose(f); +} + +static void rtc_backup_save(void) +{ + char path[512]; + rtc_backup_path(path, sizeof(path)); + FILE* f = fopen(path, "wb"); + if (!f) { + perror("SITL: rtc backup open"); + return; + } + if (fwrite(rtc_backup, 1, sizeof(rtc_backup), f) != sizeof(rtc_backup)) { + perror("SITL: rtc backup write"); + } + fclose(f); +} + +uint32_t get_rtc_backup_register(uint8_t idx) +{ + rtc_backup_load(); + return rtc_backup[idx & 7]; +} + +void set_rtc_backup_register(uint8_t idx, uint32_t value) +{ + rtc_backup_load(); + rtc_backup[idx & 7] = value; + rtc_backup_save(); +} + +void setup_portpin(uint16_t portpin, bool enable) +{ + (void)portpin; + (void)enable; +} + +#endif // DRONECAN_SUPPORT && MCU_SITL diff --git a/Src/dshot.c b/Src/dshot.c index ed82d2fc1..bc7f956f8 100644 --- a/Src/dshot.c +++ b/Src/dshot.c @@ -11,6 +11,7 @@ #include "functions.h" #include "sounds.h" #include "targets.h" +#include "hwci_perf.h" #if DRONECAN_SUPPORT #include "DroneCAN/DroneCAN.h" #endif @@ -91,6 +92,9 @@ void computeDshotDMA() high_pin_count++; if (high_pin_count > 100) { dshot_telemetry = 1; +#ifdef HWCI_PERF + hwci_perf.dshot_telem_mode = 1; +#endif } } } @@ -104,6 +108,11 @@ void computeDshotDMA() if (calcCRC == checkCRC) { signaltimeout = 0; dshot_goodcounts++; +#ifdef HWCI_PERF + hwci_perf.dshot_rx_good++; + hwci_perf.dshot_telem_mode = (uint8_t)dshot_telemetry; + hwci_perf.dshot_edt_mode = dshot_extended_telemetry; +#endif if (dpulse[11] == 1) { send_telemetry = 1; } @@ -212,10 +221,16 @@ void computeDshotDMA() if (EDT_ARM_ENABLE == 1) { EDT_ARMED = 1; } +#ifdef HWCI_PERF + hwci_perf.dshot_edt_mode = 1; +#endif break; case 14: dshot_extended_telemetry = 0; send_EDT_deinit = 1; +#ifdef HWCI_PERF + hwci_perf.dshot_edt_mode = 0; +#endif break; case 20: forward = 1 - eepromBuffer.dir_reversed; @@ -234,6 +249,9 @@ void computeDshotDMA() } } else { dshot_badcounts++; +#ifdef HWCI_PERF + hwci_perf.dshot_rx_bad++; +#endif programming_mode = 0; } } @@ -242,6 +260,10 @@ void computeDshotDMA() void make_dshot_package(uint16_t com_time) { uint16_t extended_frame_to_send = 0; +#ifdef HWCI_PERF + /* Snapshot the period the reply will advertise (stopped → 65535 later). */ + const uint16_t _hwci_com_snap = com_time; +#endif if (dshot_extended_telemetry) { // Only send extended telemetry if last frame wasn't extended. This ensures eRPM interleaving. @@ -343,4 +365,13 @@ void make_dshot_package(uint16_t com_time) } gcr[buffer_padding] = 0; #endif +#ifdef HWCI_PERF + hwci_perf.dshot_tx_frames++; + /* Prefer the period that was actually encoded (65535 when !running). */ + hwci_perf.dshot_last_com_us = (!running && extended_frame_to_send == 0) + ? 65535u + : _hwci_com_snap; + hwci_perf.dshot_telem_mode = (uint8_t)dshot_telemetry; + hwci_perf.dshot_edt_mode = dshot_extended_telemetry; +#endif } diff --git a/Src/functions.c b/Src/functions.c index f8aecdbc0..7e5f5a132 100644 --- a/Src/functions.c +++ b/Src/functions.c @@ -48,25 +48,8 @@ uint32_t getAbsDif(int number1, int number2) return (uint32_t)result; } -/* - get current value of UTILITY_TIMER timer as 16bit microseconds - */ -static inline uint16_t get_timer_us16(void) { -#if defined(STMICRO) - return UTILITY_TIMER->CNT; -#elif defined(GIGADEVICES) - return TIMER_CNT(UTILITY_TIMER); -#elif defined(ARTERY) - return UTILITY_TIMER->cval; -#elif defined(NXP) - //Return nothing since NXP micro-tick works differently - return 0; -#elif defined(WCH) - return UTILITY_TIMER->CNT>>1; -#else -#error unsupported MCU -#endif -} +/* get_timer_us16() lives in Inc/functions.h so other units (e.g. the HWCI_PERF + * instrumentation) share the same per-family utility-timer access. */ /* delay by microseconds, max 65535 diff --git a/Src/hwci_perf.c b/Src/hwci_perf.c new file mode 100644 index 000000000..3b0889e36 --- /dev/null +++ b/Src/hwci_perf.c @@ -0,0 +1,63 @@ +/* + * hwci_perf.c - Hardware-CI performance instrumentation for AM32 + * + * See Inc/hwci_perf.h for the rationale and the struct-layout contract. + * + * The whole translation unit is empty unless HWCI_PERF is defined, so it is + * safe to leave in the common source list (the Makefile globs the Src + * directory); default builds emit nothing from it. + */ +#include "hwci_perf.h" + +#ifdef HWCI_PERF + +/* + * The single instrumentation struct. Kept volatile so the compiler never + * caches fields (the debugger reads them asynchronously over SWD) and never + * elides the storage. The host finds it by the ELF symbol "hwci_perf". + * + * Min/extreme accumulators are seeded so the first real sample replaces them. + */ +volatile hwci_perf_t hwci_perf = { + .magic = HWCI_PERF_MAGIC, + .version = HWCI_PERF_VERSION, + .size = (uint16_t)sizeof(hwci_perf_t), + .ctrl_period_us_min = 0xFFFFu, + .host_cmd = HWCI_CMD_NONE, +}; + +/* + * Clear the sticky min/max accumulators so worst-case timing can be measured + * for a single test run without power-cycling the ESC. Called for the host + * HWCI_CMD_RESET_STATS command, and automatically on the armed 0->1 edge + * (the arming tune blocks the control loop for ~300 ms with IRQs off, so the + * 16-bit timestamps recorded around it alias to garbage that must not leak + * into a run's maxima regardless of when the host issues its reset). + */ +void hwci_perf_reset_stats(void) +{ + hwci_perf.ctrl_exec_us_max = 0; + hwci_perf.ctrl_period_us_max = 0; + hwci_perf.ctrl_period_us_min = 0xFFFFu; + hwci_perf.main_loop_us_max = 0; + hwci_perf.commutation_interval_max = 0; + hwci_perf.zc_jitter_max = 0; +} + +/* + * Service a host command. Called from the main loop (low priority) when + * host_cmd is non-zero. Always clears host_cmd so the command fires once. + */ +void hwci_perf_apply_cmd(void) +{ + switch (hwci_perf.host_cmd) { + case HWCI_CMD_RESET_STATS: + hwci_perf_reset_stats(); + break; + default: + break; + } + hwci_perf.host_cmd = HWCI_CMD_NONE; +} + +#endif /* HWCI_PERF */ diff --git a/Src/main.c b/Src/main.c index fc22dfae9..0b40438ad 100644 --- a/Src/main.c +++ b/Src/main.c @@ -227,6 +227,7 @@ an settings option) #include "phaseouts.h" #include "serial_telemetry.h" #include "kiss_telemetry.h" +#include "hwci_perf.h" #include "signal.h" #include "sounds.h" #include "targets.h" @@ -248,7 +249,7 @@ an settings option) #include "DroneCAN/DroneCAN.h" #endif -#include +#include "version.h" void zcfoundroutine(void); @@ -344,7 +345,7 @@ uint16_t low_cell_volt_cutoff = 330; // 3.3volts per cell //=========================== END EEPROM Defaults =========================== -const char filename[30] __attribute__((section(".file_name"))) = FILE_NAME; +const char filename[30] AM32_FLASH_SECTION(".file_name") = FILE_NAME; _Static_assert(sizeof(FIRMWARE_NAME) <=13,"Firmware name too long"); // max 12 character firmware name plus NULL // move these to targets folder or peripherals for each mcu @@ -783,8 +784,19 @@ void loadEEpromSettings() if (motor_kv < 300) { low_rpm_throttle_limit = 0; } - low_rpm_level = motor_kv / 100 / (32 / eepromBuffer.motor_poles); - high_rpm_level = motor_kv / 12 / (32 / eepromBuffer.motor_poles); + // guard divisions for an erased eeprom (motor_poles 0 or 0xff), + // ARM hardware division returns 0 but it is UB in C + uint8_t rpm_level_div = 0; + if (eepromBuffer.motor_poles != 0) { + rpm_level_div = 32 / eepromBuffer.motor_poles; + } + if (rpm_level_div != 0) { + low_rpm_level = motor_kv / 100 / rpm_level_div; + high_rpm_level = motor_kv / 12 / rpm_level_div; + } else { + low_rpm_level = 0; + high_rpm_level = 0; + } } reverse_speed_threshold = map(motor_kv, 300, 3000, 1000, 500); if (eepromBuffer.bi_direction){ @@ -915,6 +927,7 @@ void PeriodElapsedCallback() if (zero_crosses < 10000) { zero_crosses++; } + HWCI_PERF_ZC(); } /* @@ -1334,6 +1347,7 @@ if (!stepper_sine && armed) { void tenKhzRoutine() { // 20khz as of 2.00 to be renamed + HWCI_PERF_CTRL_ENTER(); duty_cycle = duty_cycle_setpoint; tenkhzcounter++; ledcounter++; @@ -1527,6 +1541,7 @@ void tenKhzRoutine() signaltimeout++; #endif + HWCI_PERF_CTRL_EXIT(); } void processDshot() @@ -1705,6 +1720,10 @@ void runBrushedLoop() */ static void checkDeviceInfo(void) { +#ifdef MCU_SITL + // no bootloader device info page in SITL + return; +#endif #ifdef NXP uint32_t pflashBlockBase = 0U; uint32_t pflashTotalSize = 0U; @@ -1893,6 +1912,7 @@ int main(void) #endif while (1) { + HWCI_PERF_MAIN_LOOP(); e_com_time = ((commutation_intervals[0] + commutation_intervals[1] + commutation_intervals[2] + commutation_intervals[3] + commutation_intervals[4] + commutation_intervals[5]) + 4) >> 1; // COMMUTATION INTERVAL IS 0.5US INCREMENTS #if defined(FIXED_DUTY_MODE) || defined(FIXED_SPEED_MODE) diff --git a/hwci/.gitignore b/hwci/.gitignore new file mode 100644 index 000000000..2acb5dcb4 --- /dev/null +++ b/hwci/.gitignore @@ -0,0 +1,14 @@ +# Python +.venv/ +__pycache__/ +*.py[cod] +*.egg-info/ +.pytest_cache/ + +# Run outputs (data is captured per-run; commit baselines/ only) +runs/ +rig.yaml +hwci/flightstand/_generated/ + +# pip build artifacts +build/ diff --git a/hwci/README.md b/hwci/README.md new file mode 100644 index 000000000..5fbeef11a --- /dev/null +++ b/hwci/README.md @@ -0,0 +1,354 @@ +# AM32 Hardware-CI Harness (ARK 4IN1 ESC) + +A hardware-in-the-loop test harness for improving AM32 on the **ARK 4IN1 ESC**. +It builds and flashes firmware, drives a motor through a **Tyto Robotics Flight +Stand 50**, and at the same time reads firmware **CPU-load / loop-time** +instrumentation off the MCU over **SWD**, then turns it all into metrics, a +baseline, and a pass/fail report you can gate pull requests on. + +``` + ┌───────────────────────── Ubuntu 24.04 host ─────────────────────────┐ + │ │ + ST-Link ◄─SWD─┤ OpenOCD ──background RAM reads──► hwci_perf struct (loop µs, iters) │ + │ │ + USB-serial ◄───┤ KISS telemetry reader ◄──────── ESC telem wire (eRPM, V, A, °C) │ + │ │ + Flight Stand◄──┤ gRPC client ──throttle──► ESC signal ──measures──► thrust/torque/│ + (gRPC) │ RPM/V/A → efficiency g/W │ + │ │ + │ runner → metrics → baseline compare → report.md + plots + exit code │ + └───────────────────────────────────────────────────────────────────────┘ + ARK 4IN1 ESC (4× STM32F051 / Cortex-M0) ── motor ── prop ── Flight Stand 50 +``` + +## Two exclusive bench setups + +| | **SETUP A — Flight Stand** | **SETUP B — ARK FPV BDShot** | +|--|----------------------------|------------------------------| +| ESC signal | Stand ESC out (uni DShot) | FPV motor out (BDShot) | +| Motor command | `hwci run --profile noprop_…` | `scripts/px4_motor_stream.py` | +| Rig file | `rig.yaml` / `config/rig.flightstand.yaml` | `config/rig.px4_bdshot.yaml` | +| Docs | this README | [docs/BENCH_SETUPS.md](docs/BENCH_SETUPS.md), [docs/setup_px4_bdshot.md](docs/setup_px4_bdshot.md) | + +Use **one setup at a time** (only one host on the signal wire). Full table and +switch procedure: **[docs/BENCH_SETUPS.md](docs/BENCH_SETUPS.md)**. + +## Why it's built this way (read this first) + +The ARK 4IN1 runs **four independent STM32F051 MCUs** (one per channel), each a +**Cortex-M0 (ARMv6-M)**. The M0 has **no DWT cycle counter, no ITM, and no SWO** +— so the usual "profile over SWO/ITM trace" approach is **impossible on any +debug probe**, ST-Link or J-Link alike. + +So CPU load and loop times are recovered a different way: + +1. The firmware keeps a tiny instrumentation struct (`hwci_perf`) in RAM, + updated from the 20 kHz control loop and the main loop using the existing + 1 µs free-running timer (`UTILITY_TIMER`/TIM17). +2. The debugger reads that struct **without halting the core** via SWD + background memory access (works on the M0; it's the same mechanism as IDE + "live watch"). Both ST-Link (OpenOCD) and J-Link support it; **ST-Link + + OpenOCD is the default** because you already use it to flash the bootloader + and the OpenOCD config already ships in the repo. +3. **CPU load** uses the idle-residual method: the firmware exposes a + free-running `loop_iters` counter whose rate (iters/s) is highest when the + core is least loaded, so `cpu_load = 1 − rate/idle_rate`. + +Efficiency and demag come from the thrust stand + ESC telemetry, correlated on +the host. Demag/desync is detected from RPM/thrust collapse at high throttle, +ESC-eRPM vs stand-RPM divergence, commutation-interval spikes, and the firmware +bemf-timeout flag. + +## What it measures + +| Channel | Source | Metrics | +|---|---|---| +| CPU load | `hwci_perf.loop_iters` via SWD | % load vs idle baseline, per operating point | +| Loop times | `hwci_perf` ctrl/main timers via SWD | worst-case 20 kHz exec µs, period jitter, main-loop µs | +| Efficiency | Flight Stand thrust + V·A | thrust (gf), electrical power (W), **g/W** per throttle | +| Demag | stand RPM + KISS eRPM + perf | desync events, eRPM/RPM mismatch, commutation spikes | +| Health | KISS telemetry | voltage, current, temperature, consumption | + +## Firmware instrumentation (`HWCI_PERF`) + +The instrumentation is **opt-in and zero-cost when off**. Build it with: + +``` +make ARK_4IN1_F051 HWCI_PERF=1 +``` + +* When `HWCI_PERF` is **unset** (all production/release builds), every hook + expands to nothing — the binary is byte-for-byte identical. +* When **set**, it adds ~**530 B flash** and **72 B RAM** on the F051 and emits + the `hwci_perf` symbol the host locates via the ELF (address + DWARF layout, + so host and firmware can never silently disagree — there's a test for it). + +Files: `Inc/hwci_perf.h`, `Src/hwci_perf.c`, three hooks in `Src/main.c` +(`tenKhzRoutine` enter/exit, `while(1)` top), one `Makefile` flag. + +## Hardware setup + +### Debug (one channel at a time) + +The ARK 4IN1 exposes all four SWD pairs on a single 10-pin debug header: + +| pin | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | +|----|---|---|---|---|---|---|---|---|---|----| +| sig | 3V3 | SWDIO1 | SWCLK1 | SWDIO2 | SWCLK2 | SWDIO3 | SWCLK3 | SWDIO4 | SWCLK4 | GND | + +Wire the ST-Link to the SWDIO/SWCLK pair of the channel whose motor is on the +stand (channel 1 by convention). **Do not** connect the ST-Link 3V3 when the ESC +is powered from a battery/supply — share GND only. All four dies run identical +firmware, so one channel's loop-time/CPU data is representative; efficiency is +per-motor. + +### Signal, telemetry, power + +* **Throttle (SETUP A)**: Flight Stand ESC output → channel-1 signal pin + (uni DShot/PWM). Optional: `throttle_backend: external` serial bridge. +* **Throttle (SETUP B)**: ARK FPV BDShot owns the pin; harness uses + `throttle_backend: none` + `scripts/px4_motor_stream.py`. Do not attach + stand ESC out and FPV motor out to the same signal pin. +* **ESC telemetry**: optional KISS wire → USB-serial (SETUP A or B). BDShot + eRPM does not use this wire (SETUP B returns eRPM on the signal line). +* **Power**: bench supply or battery within the ARK 4IN1's 3–8S range; common + ground between supply, ESC, stand, and host. +* **Safety**: set conservative cutoffs in the profile `safety:` block (current, + thrust, RPM, voltage, temperature) — the **runner enforces them host-side on + every sample** (stand reading and ESC telemetry alike) and aborts the run on + breach. Also configure the Flight Stand Software's own UI cutoffs as an + independent second layer (the vendor set-limit RPC is not mapped yet). Secure + the motor/prop; use a prop guard; keep clear during demag-stress runs. +* **Battery**: pass `--battery-cells N` to `hwci run`/`hwci ci` (e.g. `6` for a + 6S pack) to refuse to *start* a test when the pack is already at or below + `N * --min-cell-voltage` (default 3.3 V/cell, matching AM32 firmware's own + low-voltage-cutoff default). This is a pre-flight gate checked once before + the throttle is armed — separate from, and in addition to, the per-sample + `safety:` limits above. It reads the stand's voltage channel, or the perf + struct on a stand-less bench. Opt-in and hardware-only (has no effect under + `--sim`, since the simulator's pack has no real cell count). + +## Software setup (Ubuntu 24.04) + +```bash +cd hwci +./scripts/setup_ubuntu.sh # apt deps, ARM toolchain, OpenOCD, udev, venv +``` + +Then, manually (vendor bits the script can't automate): + +1. The **Tyto Flight Stand Software runs on Windows only** (no Linux build as + of v2.4.x). Install it on a Windows PC on the bench network, plug the + stand's USB into that PC, and launch the software with the `--remote` + flag so the gRPC API accepts non-local connections. Point `stand_host` + in `rig.yaml` at that PC. (The GUI is also where you configure the + vendor-side safety cutoffs. Load cells don't need a manual tare before + each run: `hwci run`/`hwci ci` tare automatically as the last pre-flight + step — with the ESC signal already up at zero throttle, because AM32 + beeps the motor whenever it has no input signal and those beeps shake + the cells mid-tare. `--no-tare` skips it.) +2. Clone Tyto's API repo — it ships **pre-compiled Python stubs**, no protoc + run needed — and make it importable in the harness venv: + ```bash + git clone https://gitlab.com/TytoRobotics/flightstand-api ~/flightstand-api + .venv/bin/pip install grpcio protobuf + echo ~/flightstand-api/languages/python > \ + .venv/lib/python3*/site-packages/flightstand_api.pth + ``` + `hwci/hwci/flightstand/grpc_client.py` is mapped against + `flight_stand_api_v1.proto` (Flight Stand Software 2.4.x); the + proto-aware calls stay isolated in `_StubAdapter` if Tyto's API moves. +3. `cp config/rig.example.yaml rig.yaml` and edit ports/host/signal map/motor. +4. Edit `/etc/udev/rules.d/99-hwci.rules` with your USB-serial serial numbers so + `/dev/esc-telem` (and `/dev/esc-throttle`) appear. +5. ESC telemetry only streams if the AM32 EEPROM setting + `telemetry_on_interval` is enabled (AM32 configurator: "30ms telemetry"). + With it off, the KISS channel is silent even when wired correctly. On a + bench without the telemetry wire, set `telem_backend: none` — the perf + struct still reports eRPM/voltage/current/temperature over SWD. + +Verify the harness with **no hardware** at any time: + +```bash +python -m hwci selftest # runs ci_smoke in the built-in simulator +python -m pytest # 75 offline tests +``` + +## Usage + +```bash +hwci profiles # list test profiles +hwci build --config rig.yaml # make ARK_4IN1_F051 HWCI_PERF=1 +hwci flash --config rig.yaml # OpenOCD program @ 0x08001000 +hwci run --profile efficiency_sweep --config rig.yaml --battery-cells 6 --out runs/r1 +hwci analyze runs/r1 # -> metrics.json +hwci report runs/r1 --baseline baselines/ARK_4IN1_F051.json +hwci ci --profile ci_smoke --config rig.yaml --battery-cells 6 \ + --baseline baselines/ARK_4IN1_F051.json --out runs/ci # full gate +``` + +`--battery-cells` is optional but recommended on hardware runs: it refuses to +start the test at all if the pack is already too low (see Safety, below), +instead of arming and collecting a run's worth of data from a sagging supply. + +`hwci ci` builds (HWCI_PERF=1) → flashes → runs the profile → computes metrics → +compares to the baseline → writes `report.md` + `summary.png`, and **exits +non-zero on regression** so CI fails the build. + +Built-in profiles: `ci_smoke` (fast gate), `efficiency_sweep` (10–100 % +staircase, the primary baseline), `demag_step_stress` (aggressive steps). + +## Auto-tuning AM32 settings (`hwci tune`) + +Given a motor/prop on the rig, `hwci tune` automatically searches the AM32 +EEPROM settings (`advance_level`, `pwm_frequency`, `variable_pwm`, +`auto_advance`, `max_ramp`, …) for the combination that maximizes efficiency +(g/W), subject to hard constraints: no demag/desync/bemf timeouts, zero-cross +jitter not regressed vs the default settings, temperatures bounded, and +reliable startup. **No rebuild per trial**: AM32 reads its 192-byte EEprom +page once at boot, so each trial one-shot-flashes the page over SWD (+ reset) +and runs a ~28 s probe. The live page address is read from the firmware's +`eeprom_address` global (the bootloader can relocate it) and the field +offsets are cross-checked against the flashed ELF's DWARF before anything is +written. Trial blobs are seeded from the device's current page and mutate +only the tuned bytes, so version/identity bytes and rig calibration survive. + +```bash +hwci tune --spec tunes/example.yaml --config rig.yaml --battery-cells 4 \ + --out runs/tune-1 # hardware +hwci tune --spec tunes/example.yaml --sim --out runs/tune-sim # offline dry run +hwci tune --resume runs/tune-1 # continue an interrupted session +``` + +**Spec format** (`tunes/example.yaml`, strict validation — unknown keys or +parameters fail loudly): `parameters:` declares the tunable fields and their +grids (unknown-to-hwci fields can be addressed with an explicit `offset:`); +`stages:` runs coordinate sweeps (`sweep:` one parameter, others at the +incumbent, optional `refine_step` around the argmax) and A/B mode stages +(`ab_candidates:` with interleaved `repeats`); `objective:` weights the +steady probe points (points under `min_power_w` are bench noise and never +score); `constraints:` are hard disqualifiers, never traded against score. A +`constraint_only: true` sweep (e.g. `max_ramp` on the step-stress profile) +tries values in listed order and picks the first with zero failures — list +them best-first. + +**Noise handling**: same-firmware g/W spread reaches ~10 % on the bench and +the pack sags within a session, so the incumbent is re-run every +`anchors_every` trials and every score is reported raw *and* normalized to +the interpolation between surrounding anchors (cancels drift). Candidates +within `noise_floor_pct` of the best tie-break toward lower jitter, then +lower FET temperature, then closest-to-default. The finals run winner vs +default in interleaved ABBA blocks on the full efficiency sweep (plus a +startup-reliability check); the winner is confirmed only with a positive +median paired delta and zero constraint failures — otherwise +`best_settings.bin` keeps the defaults. + +**Session dir / resume**: `runs/tune-1/` holds `manifest.json` (atomically +rewritten after every trial), `spec.yaml`, `base_settings.bin`, one standard +run dir per trial under `trials/T007-advance_level_22/` (plus its +`settings.bin` + `trial.json`), and at the end `report.md`, +`best_settings.bin`, `settings_diff.md`, and — when the `plot` extra +(matplotlib) is installed — a `tune_report.pdf` (verdict, settings diff, +full default/best tunable settings, settings performance impact, per-trial +objective + ABBA-delta plots, full stage/trial tables, and high-level raw run +data from every trial). +`--resume` replays the +deterministic plan: completed trials are reused from disk, partial trial +dirs are quarantined as `*.incomplete` and redone, and the incumbent page is +re-programmed first (a crash may have left arbitrary trial settings +flashed). + +**Pack swaps**: when the resting voltage drops below +`battery_cells * pack.min_resting_cell_v`, the session checkpoints and +prompts you to swap the pack (recorded as a `pack_event`; ABBA blocks that +straddle a swap are discarded and restarted). With `--no-prompt` it exits +cleanly (code 3) instead — swap, then `--resume`. + +The settings page is also scriptable directly: + +```bash +hwci settings read --config rig.yaml --bin page.bin # dump current page +hwci settings diff --config rig.yaml --bin other.bin # exit 1 if different +hwci settings write --config rig.yaml --bin page.bin # flash + verify +``` + +## Capturing the first baseline + +Once wired and configured: + +```bash +cd hwci +# 1. Sanity-check the full path on hardware with the short profile: +hwci ci --profile ci_smoke --config rig.yaml --battery-cells 6 --out runs/smoke +# 2. Capture the performance baseline: +hwci ci --profile efficiency_sweep --config rig.yaml --battery-cells 6 --out runs/baseline +hwci baseline-save runs/baseline --out baselines/ARK_4IN1_F051.json +git add baselines/ARK_4IN1_F051.json && git commit -m "hwci: ARK 4IN1 baseline" +``` + +A `--baseline` pointing at a file that doesn't exist yet prints a warning and +skips the gate (instead of failing), so the very first CI run — including a +`save_baseline` dispatch — can bootstrap the baseline itself. + +`runs/baseline/report.md` is your benchmark: thrust & efficiency per throttle, +worst-case 20 kHz loop time, CPU load, and zero demag events on the swept ramp. +Every later change is graded against it. + +The regression gate **fails closed**: a metric that is missing/NaN (dead SWD or +telemetry channel, misconfigured backend) fails its check, and per-channel +coverage checks (`perf_coverage`, `stand_coverage`, `telem_coverage`) name the +dead channel explicitly. A green run therefore proves the instrumentation was +alive, not just that nothing compared worse. + +## CI integration + +`.github/workflows/hwci.yml` runs on a self-hosted runner labelled +`[self-hosted, hwci]` wired to the rig. It is **manual-dispatch only**: the rig +needs hands-on preparation (battery connected/charged, prop and torque arm +checked, Flight Stand Software running), so a human always starts the run — +nothing triggers from pushes, PRs, or labels, and fork code never reaches the +bench unless a maintainer explicitly enters a PR number in the dispatch form. +Pick a profile (and optionally a PR number and `save_baseline`) in the form; +the workflow serializes hardware access with a concurrency group, posts +`report.md` to the job summary, uploads the run data, and fails the job on +regression (exit 1) or abort (exit 2). Set the repo/runner variable +`HWCI_RIG_CONFIG` to the path of `rig.yaml` on the bench. + +## Repo layout + +``` +Inc/hwci_perf.h, Src/hwci_perf.c firmware instrumentation (HWCI_PERF) +hwci/hwci/perf.py, elf.py perf struct decode + ELF/DWARF symbol lookup +hwci/hwci/debugger/ OpenOCD (ST-Link) + Mock backends +hwci/hwci/flightstand/ gRPC client + simulator + base interface +hwci/hwci/esc_telem/kiss.py KISS telemetry parser +hwci/hwci/throttle/ flight-stand / external / base throttle sources +hwci/hwci/sim.py offline rig simulator (all 3 channels) +hwci/hwci/runner.py metrics.py baseline.py report.py config.py cli.py +hwci/hwci/profiles/*.yaml test profiles +hwci/tests/ 75 offline tests (sim, DWARF layout, fail-closed gating) +``` + +## Honest limitations + +* The **gRPC client** is mapped against Tyto's published + `flight_stand_api_v1.proto` (Flight Stand Software 2.4.x): inputs are + discovered by `InputType` (FORCE_FZ=11 etc.), all signals are read in one + `ListSamples` round trip, and throttle goes out via `UpdateOutput` on the + ESC output's `output_target`. Tyto guarantees no cross-version API + compatibility, so re-check `_StubAdapter` after a Flight Stand Software + update. The API reports SI units (Newtons, rad/s) — `thrust_is_grams: + false`, `rpm_is_rad_per_s: true` in the signal map. +* The **AM32 bootloader only jumps to the app when the throttle signal line + idles low at boot**. An inactive stand output can leave the line high and + park the ESC in the bootloader after every flash or power-cycle (observed + on the ARK 4IN1 bench). Live-source bring-up detects this via the perf + magic, commands zero throttle, resets the MCU, and waits for the app — + see `_ensure_app_alive` in `hwci/runner.py`. +* `HWCI_PERF` is validated for the STM32F0 (ARK 4IN1). The timestamp macro uses + the shared `get_timer_us16()` helper, so STM32/GigaDevice/Artery targets + compile with `HWCI_PERF=1`; NXP and WCH are `#error`-gated (no usable + free-running 1 µs timer). With the flag unset, every target is untouched. +* CPU load is a statistical idle-residual figure, not a per-function profile + (the M0 can't do PC sampling); it's stable and comparable run-to-run, which is + what regression gating needs. diff --git a/hwci/baselines/ARK_4IN1_F051.json b/hwci/baselines/ARK_4IN1_F051.json new file mode 100644 index 000000000..cf7036b47 --- /dev/null +++ b/hwci/baselines/ARK_4IN1_F051.json @@ -0,0 +1,202 @@ +{ + "format_version": 1, + "meta": { + "aborted": null, + "git_sha": "3f8fad7", + "mode": "hw", + "motor": "JS Technology 2306 1800KV", + "n_samples": 3700, + "perf_read_errors": 0, + "pole_pairs": 7, + "profile": "noprop_baseline", + "profile_def": { + "arm_settle_s": 3.0, + "demag_commutation_spike": 3.0, + "demag_rpm_drop_fraction": 0.25, + "description": "No-prop performance baseline: steady staircase 10% to 50% throttle. Without a prop there is no meaningful thrust/efficiency, but loop timing, CPU load, commutation health, eRPM-vs-stand-RPM agreement, and no-load current per operating point are all stable regression signals. Capped at 50% (a free 1800KV bell on 6S already turns ~22k RPM there); raise only with a load. ~40 s.\n", + "name": "noprop_baseline", + "pole_pairs": 7, + "safety": { + "max_current_a": 10.0, + "max_motor_temp_c": 80.0, + "max_rpm": 28000, + "max_thrust_n": 3.0, + "max_voltage_v": null + }, + "sample_rate_hz": 100.0, + "segments": [ + { + "duration_s": 2.0, + "label": "idle", + "ramp": false, + "steady": false, + "throttle": 0.0 + }, + { + "duration_s": 6.0, + "label": "t10", + "ramp": true, + "steady": true, + "throttle": 0.1 + }, + { + "duration_s": 6.0, + "label": "t20", + "ramp": false, + "steady": true, + "throttle": 0.2 + }, + { + "duration_s": 6.0, + "label": "t30", + "ramp": false, + "steady": true, + "throttle": 0.3 + }, + { + "duration_s": 6.0, + "label": "t40", + "ramp": false, + "steady": true, + "throttle": 0.4 + }, + { + "duration_s": 6.0, + "label": "t50", + "ramp": false, + "steady": true, + "throttle": 0.5 + }, + { + "duration_s": 3.0, + "label": "rampdn", + "ramp": true, + "steady": false, + "throttle": 0.0 + }, + { + "duration_s": 2.0, + "label": "stop", + "ramp": false, + "steady": false, + "throttle": 0.0 + } + ], + "steady_tail_fraction": 0.5 + }, + "prop": "none", + "sample_rate_hz": 100.0, + "target": "ARK_4IN1_F051", + "timestamp": "2026-07-02T14:03:21", + "wall_time_s": 37.0 + }, + "metrics": { + "demag": { + "bemf_timeout_samples": 0, + "comm_spike_samples": 0, + "esc_rpm_mismatch_samples": 0, + "event_count": 0, + "events": [], + "median_commutation_interval": 162.0, + "rpm_drop_samples": 0 + }, + "steady_points": [ + { + "cpu_load_pct": 39.7, + "ctrl_exec_us_max": 76, + "current_a": 0.069, + "eff_gf_per_w": 2.191, + "elec_power_w": 1.69, + "fet_temp_c": 25.4, + "main_loop_us_max": 138, + "motor_temp_c": 24.14, + "rpm": 3919.7, + "segment": "t10", + "throttle": 0.1, + "thrust_gf": 2.03, + "voltage_v": 24.441 + }, + { + "cpu_load_pct": 54.4, + "ctrl_exec_us_max": 70, + "current_a": 0.331, + "eff_gf_per_w": 0.438, + "elec_power_w": 8.07, + "fet_temp_c": 25.77, + "main_loop_us_max": 141, + "motor_temp_c": 24.22, + "rpm": 9357.8, + "segment": "t20", + "throttle": 0.2, + "thrust_gf": 3.6, + "voltage_v": 24.414 + }, + { + "cpu_load_pct": 46.9, + "ctrl_exec_us_max": 67, + "current_a": 0.542, + "eff_gf_per_w": 0.246, + "elec_power_w": 13.22, + "fet_temp_c": 26.42, + "main_loop_us_max": 141, + "motor_temp_c": 24.29, + "rpm": 13414.6, + "segment": "t30", + "throttle": 0.3, + "thrust_gf": 3.11, + "voltage_v": 24.386 + }, + { + "cpu_load_pct": 49.7, + "ctrl_exec_us_max": 65, + "current_a": 0.792, + "eff_gf_per_w": 0.285, + "elec_power_w": 19.29, + "fet_temp_c": 27.1, + "main_loop_us_max": 146, + "motor_temp_c": 24.39, + "rpm": 17603.1, + "segment": "t40", + "throttle": 0.4, + "thrust_gf": 2.46, + "voltage_v": 24.355 + }, + { + "cpu_load_pct": 53.2, + "ctrl_exec_us_max": 57, + "current_a": 0.975, + "eff_gf_per_w": 0.124, + "elec_power_w": 23.71, + "fet_temp_c": 27.72, + "main_loop_us_max": 136, + "motor_temp_c": 24.53, + "rpm": 21744.2, + "segment": "t50", + "throttle": 0.5, + "thrust_gf": 2.93, + "voltage_v": 24.322 + } + ], + "summary": { + "bemf_timeout_samples": 0, + "best_ctrl_period_us": 8, + "demag_events": 0, + "idle_loop_rate_hz": 58527.9, + "max_cpu_load_pct": 56.8, + "max_current_a": 3.22, + "max_fet_temp_c": 28.17, + "max_motor_temp_c": 24.69, + "max_thrust_gf": 34, + "n_samples": 3700, + "peak_efficiency_gf_per_w": 2.191, + "perf_sample_count": 3700, + "stand_sample_count": 3700, + "telem_sample_count": 0, + "worst_ctrl_exec_us": 597, + "worst_ctrl_exec_us_steady": 76, + "worst_ctrl_period_us": 599, + "worst_main_loop_us": 635, + "worst_main_loop_us_steady": 146 + } + } +} \ No newline at end of file diff --git a/hwci/baselines/ARK_4IN1_F051_HQ5136.json b/hwci/baselines/ARK_4IN1_F051_HQ5136.json new file mode 100644 index 000000000..330e1f7ba --- /dev/null +++ b/hwci/baselines/ARK_4IN1_F051_HQ5136.json @@ -0,0 +1,329 @@ +{ + "format_version": 1, + "meta": { + "aborted": null, + "battery_cells": 6, + "git_sha": "66d8c9a", + "mode": "hw", + "motor": "JS Technology 2306 1800KV", + "n_samples": 6600, + "perf_read_errors": 0, + "pole_pairs": 7, + "profile": "efficiency_sweep", + "profile_def": { + "arm_settle_s": 2.0, + "demag_commutation_spike": 3.0, + "demag_rpm_drop_fraction": 0.25, + "description": "Steady-state staircase from 10% to 100% throttle. Each step dwells long enough to settle, and the tail of each is used to compute thrust, electrical power and efficiency (g/W) plus loop time / CPU load at that operating point. This is the primary efficiency + performance baseline.\nHold time is 6s. It was temporarily 10s while the ARK 4IN1 bench had severe thrust-channel noise (sample CV up to 66%) that turned out to be prop-wake impingement on the stand's mounting plate - the 5\" disc sat entirely inside the plate footprint blowing INTO it - not vibration to be averaged away. With the prop reversed to exhaust into free air (thrust CV now 5.5-8.8%), re-analysis of four 10s-hold captures truncated to 6s showed a 3s tail changes gated points (>= 20W) by under +/-2% and leaves run-to-run repeatability statistically unchanged (worst gated-point spread 9.8% vs 9.1%), so the extra 4s/point (~40s of motor+battery per sweep) bought nothing. See Thresholds.efficiency_drop_pct in hwci/baseline.py for the matching tolerance history.\n", + "name": "efficiency_sweep", + "pole_pairs": 7, + "safety": { + "max_current_a": 40.0, + "max_motor_temp_c": null, + "max_rpm": 32000, + "max_thrust_n": 16.0, + "max_voltage_v": null + }, + "sample_rate_hz": 100.0, + "segments": [ + { + "duration_s": 2.0, + "label": "idle", + "ramp": false, + "steady": false, + "throttle": 0.0 + }, + { + "duration_s": 6.0, + "label": "t10", + "ramp": true, + "steady": true, + "throttle": 0.1 + }, + { + "duration_s": 6.0, + "label": "t20", + "ramp": false, + "steady": true, + "throttle": 0.2 + }, + { + "duration_s": 6.0, + "label": "t30", + "ramp": false, + "steady": true, + "throttle": 0.3 + }, + { + "duration_s": 6.0, + "label": "t40", + "ramp": false, + "steady": true, + "throttle": 0.4 + }, + { + "duration_s": 6.0, + "label": "t50", + "ramp": false, + "steady": true, + "throttle": 0.5 + }, + { + "duration_s": 6.0, + "label": "t60", + "ramp": false, + "steady": true, + "throttle": 0.6 + }, + { + "duration_s": 6.0, + "label": "t70", + "ramp": false, + "steady": true, + "throttle": 0.7 + }, + { + "duration_s": 6.0, + "label": "t80", + "ramp": false, + "steady": true, + "throttle": 0.8 + }, + { + "duration_s": 6.0, + "label": "t90", + "ramp": false, + "steady": true, + "throttle": 0.9 + }, + { + "duration_s": 6.0, + "label": "t100", + "ramp": false, + "steady": true, + "throttle": 1.0 + }, + { + "duration_s": 4.0, + "label": "rampdn", + "ramp": true, + "steady": false, + "throttle": 0.0 + } + ], + "steady_tail_fraction": 0.5 + }, + "prop": "HQProp 5136 R36-GR-PC", + "sample_rate_hz": 100.0, + "tared": true, + "target": "ARK_4IN1_F051", + "timestamp": "2026-07-02T18:49:37", + "wall_time_s": 65.998 + }, + "metrics": { + "demag": { + "bemf_timeout_samples": 0, + "comm_spike_samples": 0, + "esc_rpm_mismatch_samples": 0, + "event_count": 0, + "events": [], + "median_commutation_interval": 127.0, + "rpm_drop_samples": 0 + }, + "steady_points": [ + { + "cpu_load_pct": 39.8, + "ctrl_exec_us_max": 77, + "current_a": 0.18, + "eff_gf_per_w": 3.079, + "elec_power_w": 4.5, + "fet_temp_c": 26.58, + "main_loop_us_max": 148, + "motor_temp_c": 24.85, + "rpm": 3943.0, + "segment": "t10", + "throttle": 0.1, + "thrust_gf": 13.37, + "voltage_v": 24.937, + "zc_jitter_max_pct": 20.41, + "zc_jitter_pct": 3.745 + }, + { + "cpu_load_pct": 55.6, + "ctrl_exec_us_max": 70, + "current_a": 0.866, + "eff_gf_per_w": 3.841, + "elec_power_w": 21.51, + "fet_temp_c": 26.83, + "main_loop_us_max": 136, + "motor_temp_c": 24.72, + "rpm": 9007.2, + "segment": "t20", + "throttle": 0.2, + "thrust_gf": 82.35, + "voltage_v": 24.855, + "zc_jitter_max_pct": 15.05, + "zc_jitter_pct": 0.769 + }, + { + "cpu_load_pct": 49.5, + "ctrl_exec_us_max": 57, + "current_a": 1.919, + "eff_gf_per_w": 3.463, + "elec_power_w": 47.44, + "fet_temp_c": 27.29, + "main_loop_us_max": 142, + "motor_temp_c": 24.59, + "rpm": 12579.6, + "segment": "t30", + "throttle": 0.3, + "thrust_gf": 164.09, + "voltage_v": 24.721, + "zc_jitter_max_pct": 33.3, + "zc_jitter_pct": 7.104 + }, + { + "cpu_load_pct": 51.4, + "ctrl_exec_us_max": 57, + "current_a": 3.764, + "eff_gf_per_w": 3.122, + "elec_power_w": 92.18, + "fet_temp_c": 27.82, + "main_loop_us_max": 137, + "motor_temp_c": 24.57, + "rpm": 16145.8, + "segment": "t40", + "throttle": 0.4, + "thrust_gf": 287.67, + "voltage_v": 24.487, + "zc_jitter_max_pct": 36.59, + "zc_jitter_pct": 8.71 + }, + { + "cpu_load_pct": 50.1, + "ctrl_exec_us_max": 66, + "current_a": 5.972, + "eff_gf_per_w": 2.77, + "elec_power_w": 144.5, + "fet_temp_c": 28.5, + "main_loop_us_max": 134, + "motor_temp_c": 24.66, + "rpm": 18908.4, + "segment": "t50", + "throttle": 0.5, + "thrust_gf": 400.14, + "voltage_v": 24.195, + "zc_jitter_max_pct": 40.91, + "zc_jitter_pct": 4.271 + }, + { + "cpu_load_pct": 53.1, + "ctrl_exec_us_max": 52, + "current_a": 8.891, + "eff_gf_per_w": 2.479, + "elec_power_w": 211.74, + "fet_temp_c": 29.66, + "main_loop_us_max": 132, + "motor_temp_c": 24.8, + "rpm": 21459.7, + "segment": "t60", + "throttle": 0.6, + "thrust_gf": 524.88, + "voltage_v": 23.816, + "zc_jitter_max_pct": 53.96, + "zc_jitter_pct": 25.643 + }, + { + "cpu_load_pct": 54.4, + "ctrl_exec_us_max": 53, + "current_a": 12.08, + "eff_gf_per_w": 2.278, + "elec_power_w": 282.72, + "fet_temp_c": 31.22, + "main_loop_us_max": 136, + "motor_temp_c": 25.12, + "rpm": 23561.0, + "segment": "t70", + "throttle": 0.7, + "thrust_gf": 643.76, + "voltage_v": 23.404, + "zc_jitter_max_pct": 44.48, + "zc_jitter_pct": 17.408 + }, + { + "cpu_load_pct": 54.8, + "ctrl_exec_us_max": 54, + "current_a": 15.729, + "eff_gf_per_w": 2.07, + "elec_power_w": 360.98, + "fet_temp_c": 33.31, + "main_loop_us_max": 137, + "motor_temp_c": 25.39, + "rpm": 25440.8, + "segment": "t80", + "throttle": 0.8, + "thrust_gf": 747.11, + "voltage_v": 22.95, + "zc_jitter_max_pct": 32.94, + "zc_jitter_pct": 9.466 + }, + { + "cpu_load_pct": 57.3, + "ctrl_exec_us_max": 65, + "current_a": 19.915, + "eff_gf_per_w": 1.849, + "elec_power_w": 447.2, + "fet_temp_c": 35.5, + "main_loop_us_max": 151, + "motor_temp_c": 25.55, + "rpm": 27190.3, + "segment": "t90", + "throttle": 0.9, + "thrust_gf": 826.69, + "voltage_v": 22.455, + "zc_jitter_max_pct": 28.57, + "zc_jitter_pct": 9.387 + }, + { + "cpu_load_pct": 52.4, + "ctrl_exec_us_max": 63, + "current_a": 24.457, + "eff_gf_per_w": 1.593, + "elec_power_w": 536.75, + "fet_temp_c": 37.87, + "main_loop_us_max": 146, + "motor_temp_c": 26.09, + "rpm": 28766.7, + "segment": "t100", + "throttle": 1.0, + "thrust_gf": 854.82, + "voltage_v": 21.947, + "zc_jitter_max_pct": 27.23, + "zc_jitter_pct": 5.37 + } + ], + "summary": { + "bemf_timeout_samples": 0, + "best_ctrl_period_us": 6, + "demag_events": 0, + "idle_loop_rate_hz": 59049.3, + "max_cpu_load_pct": 60.7, + "max_current_a": 26.63, + "max_fet_temp_c": 39.46, + "max_motor_temp_c": 26.41, + "max_thrust_gf": 952, + "n_samples": 6600, + "peak_efficiency_gf_per_w": 3.841, + "perf_sample_count": 6600, + "stand_sample_count": 6600, + "telem_sample_count": 0, + "worst_ctrl_exec_us": 656, + "worst_ctrl_exec_us_steady": 77, + "worst_ctrl_period_us": 658, + "worst_main_loop_us": 713, + "worst_main_loop_us_steady": 151, + "worst_zc_jitter_max_pct": 53.96, + "worst_zc_jitter_pct": 25.643 + } + } +} \ No newline at end of file diff --git a/hwci/baselines/ARK_4IN1_F051_HQ5136_upstream_main.json b/hwci/baselines/ARK_4IN1_F051_HQ5136_upstream_main.json new file mode 100644 index 000000000..fcfbf6549 --- /dev/null +++ b/hwci/baselines/ARK_4IN1_F051_HQ5136_upstream_main.json @@ -0,0 +1,329 @@ +{ + "format_version": 1, + "meta": { + "aborted": null, + "battery_cells": 6, + "git_sha": "99de9b8", + "mode": "hw", + "motor": "JS Technology 2306 1800KV", + "n_samples": 6600, + "perf_read_errors": 0, + "pole_pairs": 7, + "profile": "efficiency_sweep", + "profile_def": { + "arm_settle_s": 2.0, + "demag_commutation_spike": 3.0, + "demag_rpm_drop_fraction": 0.25, + "description": "Steady-state staircase from 10% to 100% throttle. Each step dwells long enough to settle, and the tail of each is used to compute thrust, electrical power and efficiency (g/W) plus loop time / CPU load at that operating point. This is the primary efficiency + performance baseline.\nHold time is 6s. It was temporarily 10s while the ARK 4IN1 bench had severe thrust-channel noise (sample CV up to 66%) that turned out to be prop-wake impingement on the stand's mounting plate - the 5\" disc sat entirely inside the plate footprint blowing INTO it - not vibration to be averaged away. With the prop reversed to exhaust into free air (thrust CV now 5.5-8.8%), re-analysis of four 10s-hold captures truncated to 6s showed a 3s tail changes gated points (>= 20W) by under +/-2% and leaves run-to-run repeatability statistically unchanged (worst gated-point spread 9.8% vs 9.1%), so the extra 4s/point (~40s of motor+battery per sweep) bought nothing. See Thresholds.efficiency_drop_pct in hwci/baseline.py for the matching tolerance history.\n", + "name": "efficiency_sweep", + "pole_pairs": 7, + "safety": { + "max_current_a": 40.0, + "max_motor_temp_c": null, + "max_rpm": 32000, + "max_thrust_n": 16.0, + "max_voltage_v": null + }, + "sample_rate_hz": 100.0, + "segments": [ + { + "duration_s": 2.0, + "label": "idle", + "ramp": false, + "steady": false, + "throttle": 0.0 + }, + { + "duration_s": 6.0, + "label": "t10", + "ramp": true, + "steady": true, + "throttle": 0.1 + }, + { + "duration_s": 6.0, + "label": "t20", + "ramp": false, + "steady": true, + "throttle": 0.2 + }, + { + "duration_s": 6.0, + "label": "t30", + "ramp": false, + "steady": true, + "throttle": 0.3 + }, + { + "duration_s": 6.0, + "label": "t40", + "ramp": false, + "steady": true, + "throttle": 0.4 + }, + { + "duration_s": 6.0, + "label": "t50", + "ramp": false, + "steady": true, + "throttle": 0.5 + }, + { + "duration_s": 6.0, + "label": "t60", + "ramp": false, + "steady": true, + "throttle": 0.6 + }, + { + "duration_s": 6.0, + "label": "t70", + "ramp": false, + "steady": true, + "throttle": 0.7 + }, + { + "duration_s": 6.0, + "label": "t80", + "ramp": false, + "steady": true, + "throttle": 0.8 + }, + { + "duration_s": 6.0, + "label": "t90", + "ramp": false, + "steady": true, + "throttle": 0.9 + }, + { + "duration_s": 6.0, + "label": "t100", + "ramp": false, + "steady": true, + "throttle": 1.0 + }, + { + "duration_s": 4.0, + "label": "rampdn", + "ramp": true, + "steady": false, + "throttle": 0.0 + } + ], + "steady_tail_fraction": 0.5 + }, + "prop": "HQProp 5136 R36-GR-PC", + "sample_rate_hz": 100.0, + "tared": true, + "target": "ARK_4IN1_F051", + "timestamp": "2026-07-02T19:03:07", + "wall_time_s": 65.997 + }, + "metrics": { + "demag": { + "bemf_timeout_samples": 0, + "comm_spike_samples": 0, + "esc_rpm_mismatch_samples": 0, + "event_count": 0, + "events": [], + "median_commutation_interval": 129.0, + "rpm_drop_samples": 0 + }, + "steady_points": [ + { + "cpu_load_pct": 42.8, + "ctrl_exec_us_max": 78, + "current_a": 0.188, + "eff_gf_per_w": 2.673, + "elec_power_w": 4.6, + "fet_temp_c": 27.94, + "main_loop_us_max": 146, + "motor_temp_c": 25.87, + "rpm": 3843.2, + "segment": "t10", + "throttle": 0.1, + "thrust_gf": 11.76, + "voltage_v": 24.503, + "zc_jitter_max_pct": 13.6, + "zc_jitter_pct": 3.354 + }, + { + "cpu_load_pct": 56.1, + "ctrl_exec_us_max": 68, + "current_a": 0.848, + "eff_gf_per_w": 3.461, + "elec_power_w": 20.72, + "fet_temp_c": 28.14, + "main_loop_us_max": 147, + "motor_temp_c": 25.63, + "rpm": 8798.9, + "segment": "t20", + "throttle": 0.2, + "thrust_gf": 71.55, + "voltage_v": 24.438, + "zc_jitter_max_pct": 32.19, + "zc_jitter_pct": 5.608 + }, + { + "cpu_load_pct": 51.5, + "ctrl_exec_us_max": 63, + "current_a": 1.923, + "eff_gf_per_w": 3.485, + "elec_power_w": 46.77, + "fet_temp_c": 28.55, + "main_loop_us_max": 140, + "motor_temp_c": 25.52, + "rpm": 12546.5, + "segment": "t30", + "throttle": 0.3, + "thrust_gf": 162.46, + "voltage_v": 24.324, + "zc_jitter_max_pct": 32.37, + "zc_jitter_pct": 9.085 + }, + { + "cpu_load_pct": 54.1, + "ctrl_exec_us_max": 62, + "current_a": 3.677, + "eff_gf_per_w": 3.156, + "elec_power_w": 88.75, + "fet_temp_c": 29.0, + "main_loop_us_max": 145, + "motor_temp_c": 25.45, + "rpm": 16018.9, + "segment": "t40", + "throttle": 0.4, + "thrust_gf": 279.94, + "voltage_v": 24.136, + "zc_jitter_max_pct": 35.21, + "zc_jitter_pct": 5.564 + }, + { + "cpu_load_pct": 54.2, + "ctrl_exec_us_max": 56, + "current_a": 5.878, + "eff_gf_per_w": 2.797, + "elec_power_w": 140.46, + "fet_temp_c": 29.59, + "main_loop_us_max": 141, + "motor_temp_c": 25.25, + "rpm": 18745.0, + "segment": "t50", + "throttle": 0.5, + "thrust_gf": 392.79, + "voltage_v": 23.895, + "zc_jitter_max_pct": 43.2, + "zc_jitter_pct": 3.397 + }, + { + "cpu_load_pct": 56.0, + "ctrl_exec_us_max": 65, + "current_a": 8.728, + "eff_gf_per_w": 2.502, + "elec_power_w": 205.89, + "fet_temp_c": 30.6, + "main_loop_us_max": 147, + "motor_temp_c": 25.47, + "rpm": 21297.1, + "segment": "t60", + "throttle": 0.6, + "thrust_gf": 515.13, + "voltage_v": 23.59, + "zc_jitter_max_pct": 46.91, + "zc_jitter_pct": 19.219 + }, + { + "cpu_load_pct": 56.1, + "ctrl_exec_us_max": 66, + "current_a": 11.895, + "eff_gf_per_w": 2.292, + "elec_power_w": 276.6, + "fet_temp_c": 32.19, + "main_loop_us_max": 148, + "motor_temp_c": 25.82, + "rpm": 23420.8, + "segment": "t70", + "throttle": 0.7, + "thrust_gf": 633.52, + "voltage_v": 23.255, + "zc_jitter_max_pct": 35.24, + "zc_jitter_pct": 13.177 + }, + { + "cpu_load_pct": 57.9, + "ctrl_exec_us_max": 66, + "current_a": 15.612, + "eff_gf_per_w": 2.077, + "elec_power_w": 356.98, + "fet_temp_c": 34.03, + "main_loop_us_max": 158, + "motor_temp_c": 26.05, + "rpm": 25359.7, + "segment": "t80", + "throttle": 0.8, + "thrust_gf": 741.33, + "voltage_v": 22.866, + "zc_jitter_max_pct": 31.09, + "zc_jitter_pct": 9.807 + }, + { + "cpu_load_pct": 61.8, + "ctrl_exec_us_max": 77, + "current_a": 19.826, + "eff_gf_per_w": 1.87, + "elec_power_w": 444.5, + "fet_temp_c": 35.95, + "main_loop_us_max": 185, + "motor_temp_c": 26.25, + "rpm": 27119.7, + "segment": "t90", + "throttle": 0.9, + "thrust_gf": 831.39, + "voltage_v": 22.42, + "zc_jitter_max_pct": 26.62, + "zc_jitter_pct": 7.292 + }, + { + "cpu_load_pct": 54.3, + "ctrl_exec_us_max": 61, + "current_a": 24.848, + "eff_gf_per_w": 1.582, + "elec_power_w": 543.91, + "fet_temp_c": 38.0, + "main_loop_us_max": 145, + "motor_temp_c": 26.67, + "rpm": 28892.8, + "segment": "t100", + "throttle": 1.0, + "thrust_gf": 860.43, + "voltage_v": 21.89, + "zc_jitter_max_pct": 5.07, + "zc_jitter_pct": 2.12 + } + ], + "summary": { + "bemf_timeout_samples": 0, + "best_ctrl_period_us": 9, + "demag_events": 0, + "idle_loop_rate_hz": 61007.7, + "max_cpu_load_pct": 63.5, + "max_current_a": 27.19, + "max_fet_temp_c": 39.21, + "max_motor_temp_c": 26.97, + "max_thrust_gf": 953, + "n_samples": 6600, + "peak_efficiency_gf_per_w": 3.485, + "perf_sample_count": 6600, + "stand_sample_count": 6600, + "telem_sample_count": 0, + "worst_ctrl_exec_us": 565, + "worst_ctrl_exec_us_steady": 78, + "worst_ctrl_period_us": 567, + "worst_main_loop_us": 647, + "worst_main_loop_us_steady": 185, + "worst_zc_jitter_max_pct": 46.91, + "worst_zc_jitter_pct": 19.219 + } + } +} \ No newline at end of file diff --git a/hwci/baselines/README.md b/hwci/baselines/README.md new file mode 100644 index 000000000..35184f1cc --- /dev/null +++ b/hwci/baselines/README.md @@ -0,0 +1,26 @@ +# Baselines + +Committed performance baselines, one JSON per target, e.g. +`ARK_4IN1_F051.json`. Each is the metrics snapshot of a known-good run that the +hardware-CI gate compares new runs against (see `hwci baseline-save` and +`hwci ci --baseline ...`). + +These are **rig- and device-specific** (motor, prop, battery, ambient). Capture +on your own bench and commit: + +``` +hwci ci --profile efficiency_sweep --config rig.yaml --out runs/baseline +hwci baseline-save runs/baseline --out baselines/ARK_4IN1_F051.json +``` + +While no baseline file exists, `hwci ci --baseline ...` warns and skips the +gate instead of failing, so the first run (or a `save_baseline` workflow +dispatch) can bootstrap it. + +Baselines are stamped with `format_version`, the run meta (target, profile), +and per-channel sample coverage; the comparison fails on identity mismatch, on +any missing/NaN gated metric, and on collapsed channel coverage — a baseline +captured with a dead SWD or telemetry channel would otherwise gate nothing. + +Re-baseline deliberately (and note why in the commit) when the motor/prop/setup +changes or after an intentional, validated performance change. diff --git a/hwci/config/rig.example.yaml b/hwci/config/rig.example.yaml new file mode 100644 index 000000000..0dfb3ed21 --- /dev/null +++ b/hwci/config/rig.example.yaml @@ -0,0 +1,71 @@ +# Example rig configuration for the AM32 hardware-CI harness. +# Copy to rig.yaml on the test bench and edit for your wiring. +# +# A rig file always describes HARDWARE: every backend must be set to a real +# backend or the explicit "none" (channel absent on this bench). Simulator +# backends and unknown keys/values are rejected - a typo must fail loudly, +# never silently degrade a hardware run. For a fully simulated run use +# `hwci run/ci --sim` (no rig file needed). + +# --- firmware / build --- +target: ARK_4IN1_F051 # AM32 build target (FILE_NAME in Inc/targets.h) +# repo_root: /home/test/AM32 # defaults to the repo this package lives in +# obj_dir: /home/test/AM32/obj +# elf_path: # auto: newest obj/AM32__*.elf +# app_load_addr: 0x08001000 # default matches Mcu/f051 ldscript (app above bootloader) + +# --- debug probe (reads CPU load / loop times over SWD) --- +debugger_backend: openocd # openocd | none +openocd_bin: openocd +# openocd_configs: # default: interface/stlink.cfg + target/stm32f0x.cfg +# - interface/stlink.cfg # (one channel of the ARK 4IN1 10-pin debug header: +# - target/stm32f0x.cfg # wire ST-Link to the SWDIO/SWCLK pair you test) +# openocd_search_dirs: [] # extra -s dirs if using custom cfgs +# For J-Link instead, supply interface/jlink.cfg here (M0 has no SWO, so this is +# still background-memory-read based, not trace). + +# --- ESC telemetry (KISS serial, the ARK target has USE_SERIAL_TELEMETRY) --- +telem_backend: serial # serial | none +telem_port: /dev/esc-telem # udev symlink recommended (see scripts/99-hwci.rules) +telem_baud: 115200 + +# --- throttle source (what drives the ESC signal wire) --- +# SETUP A: flightstand (or external serial bridge) +# SETUP B: none — ARK FPV / PX4 owns the pin (see docs/BENCH_SETUPS.md) +throttle_backend: flightstand # flightstand | external | none +# For an external DShot/PWM generator instead: +# throttle_backend: external +# throttle_port: /dev/esc-throttle +# throttle_baud: 115200 + +# --- thrust stand (Tyto Robotics Flight Stand 50) --- +# The Flight Stand Software runs on Windows only. On a Linux bench, run it on +# a Windows PC on the same network, launched with "--remote", and set +# stand_host to that PC's IP. The signal ids below are Tyto InputType enum +# values from flight_stand_api_v1.proto; set a channel to null if your bench +# lacks that sensor. +stand_backend: grpc # grpc | none (perf-only bench: none + throttle external) +stand_host: 127.0.0.1 # machine running the Flight Stand Software +stand_port: 50051 +stand_signals: + thrust: 11 # FORCE_FZ + torque: 14 # TORQUE_MZ + rpm: 15 # ROTATION_SPEED_FREQUENCY (rad/s over the API) + voltage: 6 # VOLTAGE_HV_INPUT + current: 8 # CURRENT_HALL_CURRENT + esc_output: 0 # index into ESC-type outputs, sorted by name + esc_min: 1000.0 # raw value of the lowest real throttle step + esc_max: 2000.0 # raw value of full throttle + # esc_zero: 0.0 # REQUIRED for DShot: AM32 arms only on DShot 0, + # and 1-47 are commands that must never be + # emitted (DShot: esc_zero 0, esc_min 48, + # esc_max 2047). Omit for standard PWM. + thrust_is_grams: false # gRPC API is SI (Newtons) + rpm_is_rad_per_s: true # gRPC API reports rotation in rad/s + +# --- device under test --- +# pole_pairs is authoritative here (recorded into each run's meta and used for +# the eRPM->RPM conversion); test profiles do not carry motor properties. +motor_name: "T-Motor F60 Pro V" +pole_pairs: 7 +prop: "HQ 5x4.3x3" diff --git a/hwci/config/rig.flightstand.yaml b/hwci/config/rig.flightstand.yaml new file mode 100644 index 000000000..135bee3c2 --- /dev/null +++ b/hwci/config/rig.flightstand.yaml @@ -0,0 +1,47 @@ +# ============================================================================= +# SETUP A — Flight Stand throttle (NO PX4 / NO BDShot on the signal wire) +# ============================================================================= +# Who drives ESC signal: Flight Stand ESC output (unidirectional DShot/PWM) +# Who measures physics: Flight Stand gRPC (thrust, RPM, V, I, …) +# Who reads MCU perf: ST-Link + OpenOCD (hwci_perf) +# +# Do NOT connect ARK FPV motor outputs to the ESC signal pin while using this +# file. For BDShot / PX4, use config/rig.px4_bdshot.yaml instead. +# See docs/BENCH_SETUPS.md. +# +# Active bench copy is often ../rig.yaml (same content). Prefer an explicit +# --config path when switching setups. +# ============================================================================= + +target: ARK_4IN1_F051 + +debugger_backend: openocd +openocd_bin: openocd + +telem_backend: none +# telem_port: /dev/esc-telem +# telem_baud: 115200 + +throttle_backend: flightstand + +stand_backend: grpc +stand_host: 192.168.7.20 +stand_port: 50051 +stand_signals: + thrust: 11 + torque: 14 + rpm: 15 + voltage: 6 + current: 8 + esc_output: 0 + esc_zero: 0.0 + esc_min: 48.0 + esc_max: 2047.0 + thrust_is_grams: false + rpm_is_rad_per_s: true + motor_temp: "/boards/COM3/inputs/29" + fet_temp: "/boards/COM3/inputs/31" + +motor_name: "JS Technology 2306 1800KV" +pole_pairs: 7 +prop: "HQProp 5136 R36-GR-PC" diff --git a/hwci/config/rig.px4_bdshot.yaml b/hwci/config/rig.px4_bdshot.yaml new file mode 100644 index 000000000..4bbdae47b --- /dev/null +++ b/hwci/config/rig.px4_bdshot.yaml @@ -0,0 +1,39 @@ +# ============================================================================= +# SETUP B — ARK FPV BDShot host (NO Flight Stand throttle on the signal wire) +# ============================================================================= +# Who drives ESC signal: ARK FPV (PX4 BDShot300/600 + EDT) — NOT this harness +# Who measures eRPM: PX4 esc_status (scripts/px4_*.py) +# Who reads MCU perf: optional ST-Link + OpenOCD (hwci_perf dshot_* v6) +# +# Wire ESC signal ONLY to the FPV motor pad. Disconnect Flight Stand ESC out. +# Do NOT run noprop_smoke / efficiency profiles with this rig expecting the +# stand to spin the motor — throttle_backend is "none". +# +# Motor command examples: +# ./scripts/px4_motor_stream.py --port /dev/ttyACM2 --steps 0.12:5,0.3:6 +# ./scripts/px4_bdshot_capture.py --port /dev/ttyACM2 --duration 40 +# See docs/BENCH_SETUPS.md and docs/setup_px4_bdshot.md. +# ============================================================================= + +target: ARK_4IN1_F051 + +# Flash + SWD instrumentation while PX4 owns the signal pin. +debugger_backend: openocd +openocd_bin: openocd + +# KISS serial is optional; PX4 may already consume telem on its own UART. +telem_backend: none + +# Critical: harness does not drive throttle (PX4 does). +throttle_backend: none + +# Stand sensors optional. Default none so a pure FPV+battery bench works. +# Set stand_backend: grpc only if you want optical RPM/thrust while the +# signal wire is still on the FPV (stand ESC output must stay disconnected). +stand_backend: none +# stand_host: 192.168.7.20 +# stand_port: 50051 + +motor_name: "JS Technology 2306 1800KV" +pole_pairs: 7 +prop: "none (free-run / noprop BDShot)" diff --git a/hwci/docs/BENCH_SETUPS.md b/hwci/docs/BENCH_SETUPS.md new file mode 100644 index 000000000..3304992a8 --- /dev/null +++ b/hwci/docs/BENCH_SETUPS.md @@ -0,0 +1,81 @@ +# Two exclusive bench setups + +This harness supports **two different physical setups**. Use **one at a time** — +only one device may drive the ESC signal wire. + +| | **SETUP A — Flight Stand** | **SETUP B — ARK FPV BDShot** | +|--|----------------------------|------------------------------| +| **Who drives ESC signal** | Flight Stand ESC output (uni DShot/PWM) | ARK FPV motor out (BDShot300/600 + EDT) | +| **PX4** | Not in the path | Required (USB MAVLink) | +| **BDShot replies** | No (stand is not a BDShot master) | Yes (eRPM / EDT on signal wire) | +| **Thrust / torque / optical RPM** | Flight Stand gRPC | Optional (stand sensors only; pin disconnected from stand ESC out) | +| **SWD `hwci_perf`** | Yes (ST-Link) | Optional (same ST-Link) | +| **Primary rig file** | `rig.yaml` or `config/rig.flightstand.yaml` | `config/rig.px4_bdshot.yaml` | +| **How to command motor** | `hwci run --profile noprop_…` | `scripts/px4_motor_stream.py` (+ capture) | +| **Typical profiles** | `noprop_smoke*`, `efficiency_sweep`, demag, … | `bdshot_smoke` (SWD log only; throttle is PX4) | + +## SETUP A — Flight Stand throttle (no PX4 / no BDShot) + +``` + Linux host ──gRPC──► Flight Stand ──ESC signal (uni DShot)──► AM32 + │ │ + └──SWD (ST-Link)─────┴── optional optical RPM / thrust / current +``` + +- Wire ESC **signal** to the Flight Stand ESC output only. +- Do **not** connect ARK FPV motor outputs to that pin at the same time. +- Config: see `config/rig.flightstand.yaml` (active bench file is usually `rig.yaml`). +- Examples: + +```bash +cd hwci +# flash + free-run smoke (stand drives throttle) +.venv/bin/python -m hwci flash --config rig.yaml --bin ../obj/AM32_ARK_4IN1_F051_*.bin +.venv/bin/python -m hwci run --config rig.yaml --profile noprop_smoke_100pct_3a --out runs/stand-smoke-1 +``` + +## SETUP B — ARK FPV BDShot (no Flight Stand throttle) + +``` + Linux host ──USB──► ARK FPV (PX4) ──BDShot+EDT signal──► AM32 + │ │ + │ └── optional KISS serial telem UART + └──SWD (ST-Link) optional hwci_perf dshot_* counters +``` + +- Wire ESC **signal** to the FPV motor pad only (not stand ESC out). +- Flight Stand may stay powered for **sensors** only if the signal pin is free; + typically leave stand throttle disconnected for this setup. +- Config: `config/rig.px4_bdshot.yaml` (`throttle_backend: none`). +- Motor command + eRPM live on PX4; scripts: + +```bash +cd hwci +# flash instrumented FW (throttle not driven by harness) +.venv/bin/python -m hwci flash --config config/rig.px4_bdshot.yaml \ + --bin ../obj/AM32_ARK_4IN1_F051_*.bin + +# drive motor via PX4 ACTUATOR_TEST (soft re-fire + ramp-down) +./scripts/px4_motor_stream.py --port /dev/ttyACM2 \ + --steps 0.12:5,0.25:5,0.40:6 --refresh 1.5 --timeout 3.0 --ramp-down 6 + +# optional: log esc_status only +./scripts/px4_bdshot_capture.py --port /dev/ttyACM2 --duration 40 -o runs/px4-cap.csv +``` + +Details: [setup_px4_bdshot.md](setup_px4_bdshot.md) (formerly `bdshot_baseline.md`). + +## Switching between setups + +1. **Power down** ESC / motor bus. +2. **Move the signal wire** (stand ESC out ↔ FPV motor pad). Never parallel both. +3. Use the matching **rig config** and **command path** from the table above. +4. For SETUP B on a battery, prefer soft ramp-down; on a 3 A brick, hard stop + can fault the supply (see PX4 docs). + +## What is shared + +- Same AM32 target / `HWCI_PERF=1` firmware (BDShot counters are zero-cost when + idle if the host never enables BDShot). +- Same ST-Link debug header. +- Same motor/ESC hardware (wiring of the **signal** pin differs). diff --git a/hwci/docs/bdshot_baseline.md b/hwci/docs/bdshot_baseline.md new file mode 100644 index 000000000..e13c4a96d --- /dev/null +++ b/hwci/docs/bdshot_baseline.md @@ -0,0 +1,14 @@ +# BDShot docs moved + +BDShot / ARK FPV work is **SETUP B** only: + +* Overview of both benches: [BENCH_SETUPS.md](BENCH_SETUPS.md) +* PX4 BDShot procedure: [setup_px4_bdshot.md](setup_px4_bdshot.md) +* Rig file: `config/rig.px4_bdshot.yaml` +* Motor drive: `scripts/px4_motor_stream.py` (not `hwci run` throttle) + +Flight Stand free-run / thrust tests are **SETUP A**: + +* Rig: `rig.yaml` or `config/rig.flightstand.yaml` +* Profiles: `noprop_smoke*`, `efficiency_sweep`, etc. +* Command: `hwci run --config … --profile noprop_…` diff --git a/hwci/docs/setup_px4_bdshot.md b/hwci/docs/setup_px4_bdshot.md new file mode 100644 index 000000000..f5836c3a6 --- /dev/null +++ b/hwci/docs/setup_px4_bdshot.md @@ -0,0 +1,217 @@ +# SETUP B — ARK FPV BDShot host + +> **This document is only for SETUP B.** ESC signal is driven by ARK FPV / PX4 +> (BDShot), not by the Flight Stand. For Flight Stand throttle (SETUP A), see +> [BENCH_SETUPS.md](BENCH_SETUPS.md) and `config/rig.flightstand.yaml`. +> Do not connect both hosts to the signal pin at once. + +This branch instruments AM32 so a **BDShot host** (ARK FPV + PX4) can be +correlated against SWD firmware counters and, optionally, Flight Stand RPM. + +## What was added (HWCI_PERF BDShot block) + +On `main_instrumented` this is **struct v3** (appended after v2 zc jitter). +On `feat/split-main-control` the same fields are **struct v6** (after v5 +esc_state). Host decoder keeps all historical versions. + +Firmware `hwci_perf` (build with `HWCI_PERF=1`) appends: + +| Field | Meaning | +|-------|---------| +| `dshot_rx_good` | Monotonic good-CRC DShot frames | +| `dshot_rx_bad` | Monotonic bad-CRC frames | +| `dshot_tx_frames` | BDShot reply packages built (`make_dshot_package`) | +| `dshot_last_com_us` | Period last packed into a reply (`e_com_time` units) | +| `dshot_telem_mode` | `0` = uni DShot, `1` = BDShot latched (idle-high detect) | +| `dshot_edt_mode` | Extended DShot Telemetry enable | + +Host decoder: `hwci/hwci/perf.py` (v1/v2/v3). CSV columns: `perf_dshot_*`. + +**Note:** `eepromBuffer.bi_direction` is *motor reverse / 3D mode*, not BDShot. +BDShot is the `dshot_telemetry` path (idle-high line, inverted CRC, GCR reply). + +## Layers + +``` + ARK FPV (PX4 1.17) USB CDC ── MAVLink ──► host capture script / QGC + │ + ├─ BDShot+EDT signal ──► ARK 4IN1 AM32 ──► GCR eRPM / EDT frames + ├─ serial telem UART ──► KISS V/A/°C ──► esc_status (parallel path) + └─ optional MAVLink shell (NSH) for dshot / listener + + Optional ST-Link SWD on ESC ──► hwci_perf v3 (dshot_telem_mode, rx/tx, e_rpm) +``` + +## Recommended bench setup (USB ARK FPV) + +What you described is enough for a **host-side BDShot baseline** without the +Flight Stand in the signal path: + +| Link | Role | +|------|------| +| USB (ACM) | MAVLink: `esc_status`, logs, shell | +| Motor out → ESC signal | BDShot300/600 + EDT replies | +| ESC telem pad → FC UART | Classic KISS serial telem (V/A/temp/eRPM) | +| ST-Link on ESC SWD | Optional; proves ESC latched BDShot (`dshot_telem_mode`) | +| NSH / debug shell | Very useful — see below | + +### PX4 config checklist (1.17) + +1. **Actuators** → motor protocol **BDShot300** or **BDShot600** (not plain DShot). +2. Motor **pole count** matches the bench motor (e.g. 14 poles → 7 pairs). +3. **EDT**: enabled with BDShot on builds that support it (1.16+); confirms as + non-zero temp/voltage/current in `esc_status` once spinning. +4. **Serial telem**: assign the UART in parameters / Actuators telemetry pin; + baud 115200 is typical for AM32 KISS. This is a *second* path — useful to + cross-check EDT vs wire telem, not a substitute for BDShot RPM rate. +5. Disarm safety / prop removed (or no-prop free-run) before motor test. + +### Is the MAVLink / NSH shell helpful? + +**Yes — use it.** Over USB you usually get both: + +- **MAVLink** on the CDC ACM (QGC, mavproxy, capture script) +- **NSH** via `mavlink shell` (or a second ACM / UART if you wire the + console) + +Useful shell commands while a motor test runs: + +```text +listener esc_status +listener esc_status -n 20 # a few samples +dshot status # if the dshot module exposes it +dshot esc_info -m 1 # may need telem; AM32 wants cmd ×6 +work_queue status +``` + +Motor spin without QGC (examples vary by board; QGC Motor Test is safer): + +```text +# Prefer QGC Actuators → Motor Test for first bring-up. +# Shell actuator tests differ by PX4 version — if unsure, use QGC. +``` + +Wire the **console UART** only if USB MAVLink shell is flaky; for this work +USB MAVLink + `mavlink shell` is usually enough. + +### Capture on the PC + +```bash +# Identify which ACM is PX4 (plug FPV USB, unplug other CDC gadgets if confused) +cd hwci +./scripts/px4_bdshot_capture.py --port /dev/ttyACM0 --discover 3 + +# Log while you motor-test from QGC (prop off!) +./scripts/px4_bdshot_capture.py --port /dev/ttyACM0 --duration 40 \ + -o runs/bdshot_px4_$(date +%Y%m%d_%H%M%S).csv +``` + +While logging, step throttle 0 → 20% → 50% → 80% → 0 in Motor Test. + +Pass/fail (host-only): + +| Check | Pass | +|-------|------| +| HEARTBEAT on ACM | link up | +| `ESC_STATUS` while spinning | BDShot and/or serial telem alive | +| RPM rises with throttle | decode + poles plausible | +| RPM → 0 at zero throttle | no stuck feedback | +| voltage/current/temp non-zero | EDT and/or serial telem path | + +If RPM works but V/A/temp stay zero: BDShot eRPM OK, EDT/serial not configured. +If nothing in `ESC_STATUS`: wrong protocol (plain DShot), wrong motor index, or +not spinning. + +## Phase A — flash instrumented ESC (optional SWD truth) + +1. Flash instrumented image: + ```bash + make ARK_4IN1_F051 HWCI_PERF=1 + # flash channel under test via OpenOCD / hwci flash + ``` +2. With PX4 driving BDShot, SWD should show: + - `dshot_telem_mode == 1` + - `dshot_edt_mode == 1` after EDT enable + - `dshot_rx_good` / `dshot_tx_frames` climbing + - `dshot_rx_bad` near 0 +3. Confirm on PX4: + ```text + listener esc_status + ``` + Expect non-zero RPM when spinning; zeros at disarmed/zero throttle. +4. Optional: `logger on` → step throttle → `logger off` → pull ulog. + +## Phase B — three-way correlation (SWD + PX4) + +With the ESC still on the HWCI SWD probe: + +```bash +# Terminal 1: poll SWD while PX4 drives the signal wire +cd hwci && .venv/bin/python - <<'PY' +import time +from hwci.config import load_rig +from hwci.debugger.openocd import OpenOCDDebugger +from hwci.perf_reader import PerfReader + +rig = load_rig("rig.yaml") +dbg = OpenOCDDebugger(...) # same openocd configs as hwci flash +# Or use: hwci run with a long idle profile while PX4 is the real throttle +# source — see Phase C when a px4 throttle backend exists. +PY +``` + +Practical interim approach: + +1. Disconnect Flight Stand ESC output from the signal pin. +2. Connect **PX4 motor output** → ESC signal (common GND). +3. Keep ST-Link on channel-1 SWD. +4. Drive motor from PX4; watch live: + + ```bash + cd hwci + .venv/bin/python -m hwci flash --config rig.yaml --bin ../obj/AM32_ARK_4IN1_F051_*.bin + # Use OpenOCD live mem, or a short custom poller reading hwci_perf + ``` + +Expected SWD under true BDShot: + +| Field | Expectation | +|-------|-------------| +| `dshot_telem_mode` | **1** after arm (idle-high auto-detect) | +| `dshot_rx_good` | Rate ≈ PX4 DShot rate (e.g. ~1 kHz) | +| `dshot_rx_bad` | Near 0; rising bad ⇒ line/CRC/load issue | +| `dshot_tx_frames` | Tracks RX when armed (reply each frame) | +| `e_rpm` | Matches PX4 `esc_status` / stand RPM within poles | + +If `dshot_telem_mode` stays **0**, the host is still uni-directional DShot +(or the line is not idle-high between frames). + +## Phase C — automated profile (later) + +- Profile stub: `bdshot_smoke` (throttle steps for free-run). +- Still needs a `throttle_backend: px4` (MAVLink motor test / actuator) to + fully automate; until then use Phase A/B with this firmware baseline. +- SITL already covers protocol decode: `Mcu/SITL/tests/test_dshot.py` + (`test_dshot600_bidir_edt`). + +## Size impact (F051 + HWCI_PERF) + +v2 → v3 adds **16 bytes RAM** for the new fields (struct 80 → 96). Flash +delta is a few increments in `dshot.c` only when `HWCI_PERF=1`. + +## Bench PSU safety (abrupt stop) + +An **abrupt motor stop** (ACTUATOR_TEST timeout expiry, hard cmd→0, or +high-rate command thrash that kills spin) can back-feed / spike the 3 A +bench supply and put it into **fault mode**. Then subsequent runs show +`servo_raw` changing but **RPM=0** until the supply is reset. + +Rules for PX4 motor tests on this bench: + +1. Re-fire `ACTUATOR_TEST` every ~2 s (`timeout=3`) so the ~3 s cap never + hard-cuts mid-hold. +2. Do **not** stream COMMAND_LONG at 10–50 Hz. +3. **Ramp down** over several seconds at end of run; never jump high→0. +4. If the supply faults, reset it before the next spin test. + +Script: `hwci/scripts/px4_motor_stream.py` (refresh + slew + ramp-down). diff --git a/hwci/hwci/__init__.py b/hwci/hwci/__init__.py new file mode 100644 index 000000000..52e7d9b0e --- /dev/null +++ b/hwci/hwci/__init__.py @@ -0,0 +1,12 @@ +"""AM32 hardware-in-the-loop CI harness for the ARK 4IN1 ESC. + +This package builds and flashes AM32 firmware, drives a motor on a Tyto Robotics +Flight Stand, and simultaneously reads firmware timing/CPU-load instrumentation +off the STM32F051 (Cortex-M0) over SWD, then turns the captured data into +metrics, baselines and pass/fail reports. + +See hwci/README.md for the architecture and the rationale behind reading an +instrumented RAM struct (the M0 has no SWO/ITM/DWT trace hardware). +""" + +__version__ = "0.1.0" diff --git a/hwci/hwci/__main__.py b/hwci/hwci/__main__.py new file mode 100644 index 000000000..dbdd06617 --- /dev/null +++ b/hwci/hwci/__main__.py @@ -0,0 +1,6 @@ +import sys + +from .cli import main + +if __name__ == "__main__": + sys.exit(main()) diff --git a/hwci/hwci/baseline.py b/hwci/hwci/baseline.py new file mode 100644 index 000000000..e5dcc55c2 --- /dev/null +++ b/hwci/hwci/baseline.py @@ -0,0 +1,238 @@ +"""Baseline storage and regression comparison. + +Comparison FAILS CLOSED: a gated metric that is missing, ``None``, or ``NaN`` +on either side fails its check. A dead instrumentation channel (loose SWD +cable, unplugged telemetry wire, misconfigured backend) produces empty metrics, +and an empty metric must read as "cannot prove no regression", never as PASS. +Channel-coverage checks additionally verify that every channel that was alive +when the baseline was captured is still alive in the current run. +""" +from __future__ import annotations + +import json +import math +from dataclasses import dataclass, asdict +from pathlib import Path + +FORMAT_VERSION = 1 + +# Instrumentation channels whose per-run sample coverage is gated. +_CHANNELS = ("perf", "stand", "telem") + + +@dataclass +class Thresholds: + """Pass/fail gates relative to the baseline.""" + # peak & per-point g/W may drop this much. History, all measured on the + # ARK 4IN1 + JS Technology 2306 1800KV + HQProp 5136 bench: + # - 3% initial guess, then 15%, then 25%: successive same-firmware + # repeatability checks kept swinging 8-20% per point. Root cause + # turned out to be prop-wake impingement on the stand's mounting + # plate (5" disc entirely inside the plate footprint, wake blowing + # into it) - cancelling ~75% of true thrust and buffeting the load + # cell (per-sample CV 55-66%). + # - back to 15% after the prop was reversed to exhaust into free air: + # four interleaved captures (2 firmwares x 2 runs) show worst + # gated-point (>= 20W) run-to-run spread of 9.8%, so 15% is ~1.5x + # the observed worst case. Tighten further only with more repeat + # captures demonstrating headroom. + efficiency_drop_pct: float = 15.0 + # g/W below this magnitude is load-cell noise (no-prop rig): efficiency is + # not a meaningful signal there and is not gated (the check reports "not + # gated" instead of flapping on noise around zero). + efficiency_floor_gf_per_w: float = 0.5 + # Below this baseline electrical power, thrust/power is dominated by + # measurement noise even WITH a prop (observed: 10% throttle at 2.3W + # swung -73% run-to-run; 20% throttle at 19W swung -20%) - the ratio of + # two small noisy numbers, not a meaningful efficiency figure. Applies to + # per-point checks AND to which points count toward "peak efficiency" + # (excluding them keeps peak from being hijacked by the noisiest point). + efficiency_min_power_w: float = 20.0 + ctrl_exec_increase_pct: float = 15.0 # worst control-loop exec time + # Absolute slack for the loop-time gates: allowed even when the relative + # gate is tighter (a 20us baseline must not fail on +4us of jitter). + ctrl_exec_abs_us_slack: float = 45.0 + cpu_load_increase_pts: float = 10.0 # percentage-POINT increase allowed + main_loop_increase_pct: float = 25.0 + allow_new_demag: bool = False # demag_events must not exceed baseline + min_coverage_fraction: float = 0.5 # channel coverage vs baseline coverage + + +def save_baseline(metrics: dict, path: str | Path, meta: dict | None = None) -> Path: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(json.dumps( + {"format_version": FORMAT_VERSION, "meta": meta or {}, + "metrics": metrics}, indent=2, sort_keys=True)) + return path + + +def load_baseline(path: str | Path) -> dict: + return json.loads(Path(path).read_text()) + + +def _check(name, baseline, current, ok, note=""): + return {"name": name, "baseline": baseline, "current": current, + "pass": bool(ok), "note": note} + + +def _nan(x) -> bool: + return isinstance(x, float) and math.isnan(x) + + +def _missing(x) -> bool: + return x is None or _nan(x) + + +def _worse_is_lower(baseline, current, drop_pct): + """current must not fall below baseline by more than drop_pct. + + The allowance is ``abs(baseline) * pct`` (not ``baseline * pct``) so a + negative baseline compares sanely - with the naive form an identical + negative value fails its own baseline (found by self-comparing the first + hardware baseline). + """ + if _missing(baseline) or _missing(current): + return False # fail closed: cannot prove no regression + return current >= baseline - abs(baseline) * drop_pct / 100.0 + + +def _worse_is_higher(baseline, current, inc_pct, abs_slack=None): + """current must not exceed baseline by more than inc_pct (relative) or + abs_slack (absolute), whichever allows MORE - the absolute term is a + noise floor for small baselines, not a cap on the value itself.""" + if _missing(baseline) or _missing(current): + return False # fail closed + limit = baseline * (1.0 + inc_pct / 100.0) + if abs_slack is not None: + limit = max(limit, baseline + abs_slack) + return current <= limit + + +def compare(current: dict, baseline: dict, thr: Thresholds | None = None, + current_meta: dict | None = None) -> dict: + thr = thr or Thresholds() + base = baseline["metrics"] if "metrics" in baseline else baseline + cs, bs = current["summary"], base["summary"] + checks = [] + + # Identity: a baseline captured for another target/profile must not gate + # this run (renamed profiles / new board revs otherwise mis-compare). + bmeta = baseline.get("meta", {}) if isinstance(baseline.get("meta"), dict) else {} + if current_meta: + for key in ("target", "profile"): + b, c = bmeta.get(key), current_meta.get(key) + if b and c: + checks.append(_check( + f"baseline_{key}", b, c, b == c, "identities must match")) + + def _eff_check(name, b, c, power_w=None): + """g/W gate with two noise floors, both learned from the bench: + a baseline captured without a prop has efficiency values that are + load-cell noise around zero (magnitude floor), and even WITH a prop + low-power points are a ratio of two small noisy numbers (power + floor: 10% throttle at 2.3W swung -73% run-to-run on an unchanged + firmware/hardware repeat capture).""" + if not _missing(b) and abs(b) < thr.efficiency_floor_gf_per_w: + return _check(name, b, c, True, + f"baseline |g/W| < {thr.efficiency_floor_gf_per_w} " + "(no-prop noise): not gated") + if power_w is not None and not _missing(power_w) and power_w < thr.efficiency_min_power_w: + return _check(name, b, c, True, + f"baseline power {power_w:.1f}W < " + f"{thr.efficiency_min_power_w}W: not gated") + return _check(name, b, c, _worse_is_lower(b, c, thr.efficiency_drop_pct), + f"<= {thr.efficiency_drop_pct}% drop; missing fails") + + # per-point efficiency: gate every segment present in either side; a + # steady segment that vanished from the current run fails closed. Needed + # here (ahead of the peak check below) because "peak efficiency" for the + # gate is recomputed from these points, not trusted from the summary + # scalar - see that check for why. + base_pts = {p["segment"]: p for p in base.get("steady_points", [])} + cur_pts = {p["segment"]: p for p in current.get("steady_points", [])} + + # peak_efficiency_gf_per_w: recomputed from steady_points restricted to + # baseline power >= efficiency_min_power_w, NOT taken from the summary + # scalar. "Peak" is a max() over all throttle points, and a max amplifies + # whichever point is noisiest - on the bench the literal peak was always + # the lowest-power point (10% throttle, 2.3W), so gating the raw scalar + # gated pure noise every time. Falls back to the scalar for baselines + # captured before steady_points existed. + if base_pts and cur_pts: + base_gate_pts = [p for p in base_pts.values() + if not _missing(p.get("elec_power_w")) + and p["elec_power_w"] >= thr.efficiency_min_power_w] + cur_gate_pts = [p for p in cur_pts.values() + if not _missing(p.get("elec_power_w")) + and p["elec_power_w"] >= thr.efficiency_min_power_w] + b_peak = max((p["eff_gf_per_w"] for p in base_gate_pts), default=None) + c_peak = max((p["eff_gf_per_w"] for p in cur_gate_pts), default=None) + else: + b_peak = bs.get("peak_efficiency_gf_per_w") + c_peak = cs.get("peak_efficiency_gf_per_w") + checks.append(_eff_check("peak_efficiency_gf_per_w", b_peak, c_peak)) + + # Loop-time gates use the STEADY-window worst case when the baseline has + # it: the raw run-max is dominated by motor start/stop transients that + # vary 30%+ run-to-run (705 vs 950 us observed back-to-back on the bench) + # and would make the gate flap. Old baselines without the steady keys + # fall back to the run-max. + for base_key, pct, slack in ( + ("worst_ctrl_exec_us", thr.ctrl_exec_increase_pct, + thr.ctrl_exec_abs_us_slack), + ("worst_main_loop_us", thr.main_loop_increase_pct, None)): + key = (f"{base_key}_steady" + if not _missing(bs.get(f"{base_key}_steady")) else base_key) + slack_note = f" or +{slack}us" if slack is not None else "" + checks.append(_check( + key, bs.get(key), cs.get(key), + _worse_is_higher(bs.get(key), cs.get(key), pct, slack), + f"<= +{pct}%{slack_note}; missing fails")) + + # CPU load: percentage-point increase + b_cpu, c_cpu = bs.get("max_cpu_load_pct"), cs.get("max_cpu_load_pct") + cpu_ok = (not _missing(b_cpu) and not _missing(c_cpu) + and c_cpu <= b_cpu + thr.cpu_load_increase_pts) + checks.append(_check("max_cpu_load_pct", b_cpu, c_cpu, cpu_ok, + f"<= +{thr.cpu_load_increase_pts} points; missing fails")) + + # demag events + b_dem, c_dem = bs.get("demag_events"), cs.get("demag_events") + dem_ok = (thr.allow_new_demag + or (not _missing(b_dem) and not _missing(c_dem) and c_dem <= b_dem)) + checks.append(_check("demag_events", b_dem, c_dem, dem_ok, + "must not exceed baseline; missing fails")) + + # Instrumentation coverage: every channel alive at baseline capture must + # still deliver samples now, else its gates above passed vacuously... which + # they no longer do, but this check names the DEAD CHANNEL explicitly. + b_tot, c_tot = bs.get("n_samples"), cs.get("n_samples") + for chan in _CHANNELS: + b_n = bs.get(f"{chan}_sample_count") + if _missing(b_n) or not b_n or _missing(b_tot) or not b_tot: + continue # baseline (older format) has no coverage info + c_n = cs.get(f"{chan}_sample_count") + b_ratio = b_n / b_tot + c_ratio = (c_n / c_tot) if not _missing(c_n) and not _missing(c_tot) and c_tot else None + ok = c_ratio is not None and c_ratio >= thr.min_coverage_fraction * b_ratio + checks.append(_check( + f"{chan}_coverage", round(b_ratio, 3), + round(c_ratio, 3) if c_ratio is not None else None, ok, + f">= {thr.min_coverage_fraction}x baseline coverage " + f"(dead {chan} channel?)")) + + # per-point efficiency (base_pts/cur_pts computed above, ahead of the + # peak check): gate every segment present in either side; a steady + # segment that vanished from the current run fails closed. + for label, bp in base_pts.items(): + p = cur_pts.get(label) + if p is None: + checks.append(_check(f"eff@{label}", bp.get("eff_gf_per_w"), None, + False, "segment missing from current run")) + continue + checks.append(_eff_check(f"eff@{label}", bp.get("eff_gf_per_w"), + p.get("eff_gf_per_w"), bp.get("elec_power_w"))) + + passed = all(c["pass"] for c in checks) + return {"passed": passed, "checks": checks, "thresholds": asdict(thr)} diff --git a/hwci/hwci/build.py b/hwci/hwci/build.py new file mode 100644 index 000000000..9fabc46a0 --- /dev/null +++ b/hwci/hwci/build.py @@ -0,0 +1,63 @@ +"""Firmware build helpers (wrap the AM32 Makefile).""" +from __future__ import annotations + +import subprocess +from dataclasses import dataclass +from pathlib import Path + + +@dataclass +class BuildArtifacts: + elf: Path + bin: Path + hex: Path + + +def find_artifact(obj_dir: Path, target: str, ext: str) -> Path | None: + """Newest ``obj/AM32__*.`` build artifact, or None. + + The single place that knows the Makefile's artifact naming; RigConfig's + ELF resolution reuses it so the flashed binary and the parsed ELF can + never be picked by two different rules. + """ + hits = sorted(obj_dir.glob(f"AM32_{target}_*.{ext}")) + return hits[-1] if hits else None + + +def build_firmware(repo_root: str | Path, target: str, *, + hwci_perf: bool = True, jobs: int = 4, + arm_sdk_prefix: str | None = None, + extra_make_args: list[str] | None = None) -> BuildArtifacts: + """Run ``make `` (with HWCI_PERF=1 by default) and return artifacts.""" + repo_root = Path(repo_root) + cmd = ["make", target, f"-j{jobs}"] + if hwci_perf: + cmd.append("HWCI_PERF=1") + if arm_sdk_prefix: + cmd.append(f"ARM_SDK_PREFIX={arm_sdk_prefix}") + if extra_make_args: + cmd += extra_make_args + proc = subprocess.run(cmd, cwd=repo_root, capture_output=True, text=True) + if proc.returncode != 0: + raise RuntimeError( + f"firmware build failed (rc={proc.returncode}):\n" + f"{proc.stdout[-2000:]}\n{proc.stderr[-2000:]}") + obj = repo_root / "obj" + elf, binf, hexf = (find_artifact(obj, target, e) for e in ("elf", "bin", "hex")) + if elf is None or binf is None: + raise RuntimeError(f"build produced no artifacts for {target} in {obj}") + if hwci_perf: + # The Makefile does not encode HWCI_PERF in object paths, so a prior + # non-instrumented build leaves up-to-date objects and this "build" + # silently packages firmware WITHOUT the perf struct (caught on the + # bench: flashed an ELF with no hwci_perf symbol). Verify, don't hope. + try: + from . import elf as elfmod + elfmod.find_symbol(str(elf), "hwci_perf") + except ImportError: + pass # no pyelftools: PerfReader will catch it at run time + except Exception as e: + raise RuntimeError( + f"{elf} lacks the hwci_perf symbol - stale non-instrumented " + f"objects in obj/ (run 'make clean' and rebuild): {e}") from e + return BuildArtifacts(elf=elf, bin=binf, hex=hexf) diff --git a/hwci/hwci/cli.py b/hwci/hwci/cli.py new file mode 100644 index 000000000..3d7c7b8fc --- /dev/null +++ b/hwci/hwci/cli.py @@ -0,0 +1,368 @@ +"""Command-line interface for the AM32 hardware-CI harness. + +Examples +-------- + hwci profiles # list built-in test profiles + hwci selftest # run ci_smoke in the simulator + hwci run --profile efficiency_sweep --sim --out runs/sweep + hwci analyze runs/sweep + hwci baseline-save runs/sweep --out baselines/ARK_4IN1_F051.json + hwci ci --profile ci_smoke --config rig.yaml \ + --baseline baselines/ARK_4IN1_F051.json --out runs/ci + +Mode selection: a run is SIMULATED only when ``--sim`` is passed or no +``--config`` is given; a rig config always means hardware (and refuses +simulator backends), so a typo can never silently gate simulated data. +""" +from __future__ import annotations + +import argparse +import json +import subprocess +import sys +from datetime import datetime +from pathlib import Path + +from . import baseline as bl +from . import metrics as metricsmod +from . import report as reportmod +from .config import (Profile, RigConfig, list_profiles, load_profile, + load_rig, profile_from_dict, profile_to_dict) +from .model import RunResult +from .runner import (DEFAULT_MIN_CELL_VOLTAGE, build_live_sources, + build_sim_sources, run_profile) + + +def _git_sha(repo_root: str) -> str | None: + try: + out = subprocess.run(["git", "rev-parse", "--short", "HEAD"], + cwd=repo_root, capture_output=True, text=True) + return out.stdout.strip() or None + except Exception: + return None + + +def _make_meta(rig: RigConfig, profile: Profile, mode: str, + extra: dict | None = None) -> dict: + meta = { + "target": rig.target, + "profile": profile.name, + # Full profile definition: the run dir stays analyzable even if the + # profile YAML changes later (or was a custom file path). + "profile_def": profile_to_dict(profile), + "mode": mode, # "sim" | "hw" - shown in the report, never inferred later + "pole_pairs": rig.pole_pairs, + "git_sha": _git_sha(rig.repo_root), + "motor": rig.motor_name, + "prop": rig.prop, + "timestamp": datetime.now().isoformat(timespec="seconds"), + } + if extra: + meta.update(extra) + return meta + + +def _use_sim(args) -> bool: + """Simulation is explicit: --sim, or no rig config at all.""" + return bool(getattr(args, "sim", False)) or not getattr(args, "config", None) + + +def _default_out(profile_name: str, sim: bool) -> Path: + stamp = datetime.now().strftime("%Y%m%d-%H%M%S") + tag = "sim" if sim else "hw" + return Path("runs") / f"{profile_name}-{tag}-{stamp}" + + +def _profile_for(result: RunResult) -> Profile: + """The exact profile a run was made with (from meta), else by name.""" + pd = result.meta.get("profile_def") + if pd: + return profile_from_dict(pd) + return load_profile(result.meta.get("profile", "ci_smoke")) + + +def _load_baseline(path: str | None) -> dict | None: + if not path: + return None + p = Path(path) + if not p.exists(): + print(f"WARNING: baseline {p} not found - skipping the regression " + "gate (bootstrap run? capture one with 'hwci baseline-save')", + file=sys.stderr) + return None + return bl.load_baseline(p) + + +def _execute(rig: RigConfig, profile: Profile, sim: bool, *, + realtime: bool | None = None, + battery_cells: int | None = None, + min_cell_voltage: float = DEFAULT_MIN_CELL_VOLTAGE, + tare: bool = True) -> RunResult: + """Build sources, run the profile, always close sources. + + The battery and tare pre-flight steps only apply to hardware: a simulated + pack doesn't represent any real cell count and simulated load cells have + no zero drift, so neither is ever passed to the simulator sources. + """ + sources = (build_sim_sources(rig, profile) if sim + else build_live_sources(rig, profile, battery_cells=battery_cells, + min_cell_voltage=min_cell_voltage, + tare=tare)) + # Recorded so a drifting thrust offset in saved data can be told apart + # from "this run simply wasn't tared". + tared = bool(tare) and not sim and rig.stand_backend == "grpc" + try: + return run_profile( + profile, sources, + realtime=(not sim) if realtime is None else realtime, + meta=_make_meta(rig, profile, "sim" if sim else "hw", + extra={"battery_cells": battery_cells, + "tared": tared})) + finally: + sources.close() + + +def _analyze_and_report(run_dir: Path, result: RunResult, profile: Profile, + baseline_path: str | None, no_plots: bool): + """Compute metrics, write metrics.json + report.md, gate vs baseline.""" + m = metricsmod.compute(result, profile) + (run_dir / "metrics.json").write_text(json.dumps(m, indent=2)) + baseline = _load_baseline(baseline_path) + comparison = (bl.compare(m, baseline, current_meta=result.meta) + if baseline is not None else None) + reportmod.write_report(run_dir, m, comparison, result.meta, + plots=not no_plots) + return m, comparison + + +def _verdict_rc(result: RunResult, comparison: dict | None) -> int: + """Exit code: 2 aborted (never trust the data), 1 gate FAIL, 0 PASS.""" + if result.meta.get("aborted"): + print(f"ABORTED: {result.meta['aborted']}", file=sys.stderr) + return 2 + if comparison is not None: + print("VERDICT:", "PASS" if comparison["passed"] else "FAIL") + return 0 if comparison["passed"] else 1 + return 0 + + +# -------------------------------------------------------------------------- +def cmd_profiles(args) -> int: + for name in list_profiles(): + p = load_profile(name) + print(f"{name:22s} {p.duration_s:5.0f}s {p.description.strip().splitlines()[0]}") + return 0 + + +def cmd_selftest(args) -> int: + rig = RigConfig() + profile = load_profile(args.profile) + sources = build_sim_sources(rig, profile, demag_prone=True) + try: + result = run_profile(profile, sources, realtime=False, + meta=_make_meta(rig, profile, "sim")) + finally: + sources.close() + m = metricsmod.compute(result, profile) + print(json.dumps(m["summary"], indent=2)) + print(f"\n{len(result.rows)} samples, " + f"{len(m['steady_points'])} steady points, " + f"{m['demag']['event_count']} demag events") + return 0 + + +def cmd_build(args) -> int: + from .build import build_firmware + rig = load_rig(args.config) + target = args.target or rig.target + arts = build_firmware(rig.repo_root, target, hwci_perf=not args.no_perf, + arm_sdk_prefix=args.arm_sdk_prefix) + print(f"built {arts.elf}\n {arts.bin}") + return 0 + + +def cmd_flash(args) -> int: + from .build import build_firmware + from .debugger.openocd import OpenOcdDebugger + rig = load_rig(args.config) + if args.bin: + binf = Path(args.bin) + else: + arts = build_firmware(rig.repo_root, rig.target, + hwci_perf=not args.no_perf, + arm_sdk_prefix=args.arm_sdk_prefix) + binf = arts.bin + dbg = OpenOcdDebugger(rig.openocd_configs, openocd_bin=rig.openocd_bin, + search_dirs=rig.openocd_search_dirs) + dbg.flash(str(binf), rig.app_load_addr) + print(f"flashed {binf} @ 0x{rig.app_load_addr:08x}") + return 0 + + +def cmd_run(args) -> int: + sim = _use_sim(args) + rig = load_rig(args.config) if args.config else RigConfig() + profile = load_profile(args.profile) + result = _execute(rig, profile, sim, + realtime=True if args.realtime else None, + battery_cells=args.battery_cells, + min_cell_voltage=args.min_cell_voltage, + tare=not args.no_tare) + out = Path(args.out) if args.out else _default_out(profile.name, sim) + result.save(out) + print(f"saved {len(result.rows)} samples to {out}") + return _verdict_rc(result, None) + + +def cmd_analyze(args) -> int: + result = RunResult.load(args.run_dir) + m = metricsmod.compute(result, _profile_for(result)) + (Path(args.run_dir) / "metrics.json").write_text(json.dumps(m, indent=2)) + print(json.dumps(m["summary"], indent=2)) + return 0 + + +def cmd_baseline_save(args) -> int: + result = RunResult.load(args.run_dir) + m = metricsmod.compute(result, _profile_for(result)) + out = args.out or f"baselines/{result.meta.get('target', 'unknown')}.json" + bl.save_baseline(m, out, meta=result.meta) + print(f"baseline saved to {out}") + return 0 + + +def cmd_report(args) -> int: + result = RunResult.load(args.run_dir) + _, comparison = _analyze_and_report( + Path(args.run_dir), result, _profile_for(result), + args.baseline, args.no_plots) + print(f"report written to {Path(args.run_dir) / 'report.md'}") + return _verdict_rc(result, comparison) + + +def cmd_ci(args) -> int: + sim = _use_sim(args) + rig = load_rig(args.config) if args.config else RigConfig() + profile = load_profile(args.profile) + out = Path(args.out) if args.out else _default_out(profile.name, sim) + + if not sim: + from .build import build_firmware + arts = build_firmware(rig.repo_root, rig.target, hwci_perf=True, + arm_sdk_prefix=args.arm_sdk_prefix) + if rig.debugger_backend == "openocd": + from .debugger.openocd import OpenOcdDebugger + dbg = OpenOcdDebugger(rig.openocd_configs, openocd_bin=rig.openocd_bin, + search_dirs=rig.openocd_search_dirs) + dbg.flash(str(arts.bin), rig.app_load_addr) + dbg.close() + else: + print("WARNING: debugger_backend is not 'openocd' - firmware was " + "built but NOT flashed; testing whatever is on the target", + file=sys.stderr) + rig.elf_path = str(arts.elf) + + result = _execute(rig, profile, sim, + battery_cells=args.battery_cells, + min_cell_voltage=args.min_cell_voltage, + tare=not args.no_tare) + result.save(out) + + m, comparison = _analyze_and_report(out, result, profile, + args.baseline, args.no_plots) + print(json.dumps(m["summary"], indent=2)) + return _verdict_rc(result, comparison) + + +def build_parser() -> argparse.ArgumentParser: + p = argparse.ArgumentParser(prog="hwci", description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter) + sub = p.add_subparsers(dest="cmd", required=True) + + def add_common(sp): + sp.add_argument("--config", help="rig config YAML (hardware run); " + "omit for the built-in simulator") + + def add_preflight(sp): + sp.add_argument("--battery-cells", type=int, metavar="N", + help="LiPo cell count under test (e.g. 6 for a 6S " + "pack); if given, refuses to start when pack " + "voltage is below N * --min-cell-voltage " + "(hardware only - ignored under --sim)") + sp.add_argument("--min-cell-voltage", type=float, metavar="V", + default=DEFAULT_MIN_CELL_VOLTAGE, + help="per-cell cutoff volts for --battery-cells " + f"(default: {DEFAULT_MIN_CELL_VOLTAGE})") + sp.add_argument("--no-tare", action="store_true", + help="skip the automatic pre-run load-cell tare " + "(hardware runs tare by default, with the ESC " + "signal held at zero throttle so AM32's " + "no-signal beacon beeps can't shake the cells " + "mid-tare)") + + sp = sub.add_parser("profiles", help="list test profiles") + sp.set_defaults(func=cmd_profiles) + + sp = sub.add_parser("selftest", help="run a profile in the simulator") + sp.add_argument("--profile", default="ci_smoke") + sp.set_defaults(func=cmd_selftest) + + sp = sub.add_parser("build", help="build firmware (HWCI_PERF=1)") + add_common(sp) + sp.add_argument("--target") + sp.add_argument("--no-perf", action="store_true", help="build without instrumentation") + sp.add_argument("--arm-sdk-prefix") + sp.set_defaults(func=cmd_build) + + sp = sub.add_parser("flash", help="flash firmware via OpenOCD") + add_common(sp) + sp.add_argument("--bin", help="bin to flash (default: build it)") + sp.add_argument("--no-perf", action="store_true") + sp.add_argument("--arm-sdk-prefix") + sp.set_defaults(func=cmd_flash) + + sp = sub.add_parser("run", help="run a profile and save the data") + add_common(sp) + add_preflight(sp) + sp.add_argument("--profile", required=True) + sp.add_argument("--sim", action="store_true", help="force the simulator") + sp.add_argument("--realtime", action="store_true", help="pace sim in real time") + sp.add_argument("--out") + sp.set_defaults(func=cmd_run) + + sp = sub.add_parser("analyze", help="compute metrics for a run dir") + sp.add_argument("run_dir") + sp.set_defaults(func=cmd_analyze) + + sp = sub.add_parser("baseline-save", help="save a run's metrics as the baseline") + sp.add_argument("run_dir") + sp.add_argument("--out") + sp.set_defaults(func=cmd_baseline_save) + + sp = sub.add_parser("report", help="write a Markdown report for a run dir") + sp.add_argument("run_dir") + sp.add_argument("--baseline") + sp.add_argument("--no-plots", action="store_true") + sp.set_defaults(func=cmd_report) + + sp = sub.add_parser("ci", help="build+flash+run+analyze+gate (full pipeline)") + add_common(sp) + add_preflight(sp) + sp.add_argument("--profile", default="ci_smoke") + sp.add_argument("--baseline") + sp.add_argument("--sim", action="store_true") + sp.add_argument("--arm-sdk-prefix") + sp.add_argument("--out") + sp.add_argument("--no-plots", action="store_true") + sp.set_defaults(func=cmd_ci) + + return p + + +def main(argv: list[str] | None = None) -> int: + args = build_parser().parse_args(argv) + return args.func(args) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/hwci/hwci/config.py b/hwci/hwci/config.py new file mode 100644 index 000000000..aa336793a --- /dev/null +++ b/hwci/hwci/config.py @@ -0,0 +1,204 @@ +"""Rig and test-profile configuration (YAML-backed). + +Rig configs from files are validated STRICTLY: unknown keys and unknown +backend values are errors, and simulator backends are rejected in a rig file. +A typo must never silently turn a hardware run into a simulated one (or drop a +channel) while CI reports green - use the explicit ``--sim`` flag to simulate. +""" +from __future__ import annotations + +import dataclasses +from dataclasses import dataclass, field +from pathlib import Path + +import yaml + +from .build import find_artifact +from .debugger.openocd import APP_LOAD_ADDR, DEFAULT_CONFIGS +from .flightstand.base import SafetyLimits + +PROFILES_DIR = Path(__file__).parent / "profiles" +DEFAULT_TARGET = "ARK_4IN1_F051" + +# Allowed backend values. "sim" entries are valid only for the built-in +# default RigConfig (offline runs); load_rig() rejects them in a rig file. +BACKEND_CHOICES: dict[str, set[str]] = { + "debugger_backend": {"openocd", "sim", "none"}, + "telem_backend": {"serial", "sim", "none"}, + # "none" = ESC signal driven outside the harness (e.g. ARK FPV BDShot); + # use only with setup B — see docs/BENCH_SETUPS.md. + "throttle_backend": {"flightstand", "external", "none", "sim"}, + "stand_backend": {"grpc", "sim", "none"}, +} +_SIM_ONLY = {"sim"} + + +@dataclass +class Segment: + """One phase of a test profile.""" + label: str + throttle: float # target throttle, 0..1 + duration_s: float + ramp: bool = False # ramp linearly from the previous throttle + steady: bool = False # use this segment for steady-state metrics + + +@dataclass +class Profile: + name: str + description: str = "" + sample_rate_hz: float = 100.0 + arm_settle_s: float = 2.0 + segments: list[Segment] = field(default_factory=list) + safety: SafetyLimits = field(default_factory=SafetyLimits) + # analysis knobs + steady_tail_fraction: float = 0.5 # use last half of a steady segment + demag_commutation_spike: float = 3.0 # x median commutation interval + demag_rpm_drop_fraction: float = 0.25 # rpm fell >25% while throttle high + # Offline fallback only: on a rig the motor's pole pairs come from + # RigConfig (recorded into the run meta), not from the test profile. + pole_pairs: int = 7 + + @property + def duration_s(self) -> float: + return sum(s.duration_s for s in self.segments) + + +def _safety_from(d: dict) -> SafetyLimits: + d = d or {} + return SafetyLimits( + max_thrust_n=d.get("max_thrust_n"), + max_current_a=d.get("max_current_a"), + max_rpm=d.get("max_rpm"), + max_voltage_v=d.get("max_voltage_v"), + max_motor_temp_c=d.get("max_motor_temp_c"), + ) + + +def profile_from_dict(d: dict) -> Profile: + segs = [Segment(label=s.get("label", f"seg{i}"), + throttle=float(s["throttle"]), + duration_s=float(s["duration_s"]), + ramp=bool(s.get("ramp", False)), + steady=bool(s.get("steady", False))) + for i, s in enumerate(d.get("segments", []))] + return Profile( + name=d["name"], + description=d.get("description", ""), + sample_rate_hz=float(d.get("sample_rate_hz", 100.0)), + arm_settle_s=float(d.get("arm_settle_s", 2.0)), + segments=segs, + safety=_safety_from(d.get("safety")), + steady_tail_fraction=float(d.get("steady_tail_fraction", 0.5)), + demag_commutation_spike=float(d.get("demag_commutation_spike", 3.0)), + demag_rpm_drop_fraction=float(d.get("demag_rpm_drop_fraction", 0.25)), + pole_pairs=int(d.get("pole_pairs", 7)), + ) + + +def profile_to_dict(profile: Profile) -> dict: + """Serialize a Profile (inverse of :func:`profile_from_dict`). + + Stored in each run's ``meta.json`` so a run directory is self-describing: + analysis re-uses the exact profile the run was made with, even if the + profile YAML changed since (or was a custom file path). + """ + return dataclasses.asdict(profile) + + +def load_profile(name_or_path: str) -> Profile: + """Load a profile by built-in name (in profiles/) or by file path.""" + p = Path(name_or_path) + if not p.exists(): + cand = PROFILES_DIR / f"{name_or_path}.yaml" + if cand.exists(): + p = cand + else: + raise FileNotFoundError( + f"profile {name_or_path!r} not found " + f"(looked in {PROFILES_DIR} and as a path)") + return profile_from_dict(yaml.safe_load(p.read_text())) + + +def list_profiles() -> list[str]: + return sorted(p.stem for p in PROFILES_DIR.glob("*.yaml")) + + +@dataclass +class RigConfig: + """How the host reaches the hardware. Defaults are the offline simulator.""" + target: str = DEFAULT_TARGET + repo_root: str = str(Path(__file__).resolve().parents[2]) + obj_dir: str | None = None # default /obj + elf_path: str | None = None # default: glob obj for the target + + debugger_backend: str = "sim" # "openocd" | "none" (| "sim" offline) + openocd_configs: list[str] = field( + default_factory=lambda: list(DEFAULT_CONFIGS)) + openocd_search_dirs: list[str] = field(default_factory=list) + openocd_bin: str = "openocd" + app_load_addr: int = APP_LOAD_ADDR + + telem_backend: str = "sim" # "serial" | "none" (| "sim" offline) + telem_port: str = "/dev/ttyUSB0" + telem_baud: int = 115200 + + # flightstand = SETUP A; external = serial bridge; none = SETUP B (PX4/BDShot owns pin) + throttle_backend: str = "sim" + throttle_port: str = "/dev/ttyACM0" + throttle_baud: int = 115200 + + stand_backend: str = "sim" # "grpc" | "none" (| "sim" offline) + stand_host: str = "127.0.0.1" + stand_port: int = 50051 + stand_signals: dict = field(default_factory=dict) + + motor_name: str = "sim-motor" + pole_pairs: int = 7 # the motor under test (authoritative) + prop: str = "sim-prop" + + def resolved_obj_dir(self) -> Path: + return Path(self.obj_dir) if self.obj_dir else Path(self.repo_root) / "obj" + + def resolved_elf(self) -> Path | None: + if self.elf_path: + return Path(self.elf_path) + return find_artifact(self.resolved_obj_dir(), self.target, "elf") + + def validate(self, *, allow_sim_backends: bool = True) -> None: + for key, choices in BACKEND_CHOICES.items(): + value = getattr(self, key) + if value not in choices: + raise ValueError( + f"rig config: {key} = {value!r} is not one of " + f"{sorted(choices)}") + if not allow_sim_backends and value in _SIM_ONLY: + raise ValueError( + f"rig config: {key} = 'sim' is not allowed in a rig file; " + "use a real backend or 'none' (or run with --sim for a " + "fully simulated run)") + if self.throttle_backend == "flightstand" and self.stand_backend == "none": + raise ValueError( + "rig config: throttle_backend 'flightstand' needs a stand " + "(stand_backend 'grpc'); use throttle_backend 'external' on a " + "stand-less bench") + + +def load_rig(path: str | None) -> RigConfig: + if not path: + return RigConfig() + data = yaml.safe_load(Path(path).read_text()) or {} + known = {f.name for f in dataclasses.fields(RigConfig)} + unknown = sorted(set(data) - known) + if unknown: + raise ValueError( + f"rig config {path}: unknown key(s) {unknown}; " + f"valid keys: {sorted(known)}") + cfg = RigConfig() + for key, value in data.items(): + setattr(cfg, key, value) + # A rig FILE describes hardware: simulator backends in it are almost + # certainly a typo'd or half-edited config, and silently simulating a + # "hardware" run is the worst possible failure mode. + cfg.validate(allow_sim_backends=False) + return cfg diff --git a/hwci/hwci/debugger/__init__.py b/hwci/hwci/debugger/__init__.py new file mode 100644 index 000000000..f83a8ab6c --- /dev/null +++ b/hwci/hwci/debugger/__init__.py @@ -0,0 +1,3 @@ +"""Debugger backends for reading firmware instrumentation over SWD.""" +from .base import Debugger, DebuggerError, MockDebugger # noqa: F401 +from .openocd import OpenOcdDebugger # noqa: F401 diff --git a/hwci/hwci/debugger/base.py b/hwci/hwci/debugger/base.py new file mode 100644 index 000000000..894b2a45a --- /dev/null +++ b/hwci/hwci/debugger/base.py @@ -0,0 +1,77 @@ +"""Debugger backend abstraction. + +A :class:`Debugger` flashes firmware and performs *background* (non-halting) +memory reads/writes over SWD while the MCU runs. That background access is the +only way to read CPU-load / loop-time data off the STM32F051, whose Cortex-M0 +core has no SWO/ITM/DWT trace hardware. + +Both the OpenOCD (ST-Link) backend and a J-Link backend would implement this +interface; :class:`MockDebugger` implements it in-process for offline tests and +simulator runs. +""" +from __future__ import annotations + +import abc + + +class DebuggerError(RuntimeError): + pass + + +class Debugger(abc.ABC): + @abc.abstractmethod + def flash(self, bin_path: str, load_addr: int) -> None: + """Program ``bin_path`` at ``load_addr`` and reset into it.""" + + @abc.abstractmethod + def read_memory(self, addr: int, length: int) -> bytes: + """Read ``length`` bytes from target RAM without halting the core.""" + + @abc.abstractmethod + def write_u32(self, addr: int, value: int) -> None: + """Write a 32-bit word to target RAM without halting the core.""" + + def reset_run(self) -> None: # optional + pass + + def close(self) -> None: # optional + pass + + def __enter__(self) -> "Debugger": + return self + + def __exit__(self, *exc) -> None: + self.close() + + +class MockDebugger(Debugger): + """In-memory debugger backed by a flat byte buffer. + + Used by the simulator and unit tests. ``base`` is the address that maps to + the start of the buffer; reads/writes outside the buffer raise. + """ + + def __init__(self, base: int = 0x20000000, size: int = 0x2000): + self.base = base + self.mem = bytearray(size) + self.flashed: list[tuple[str, int]] = [] + + # --- helpers for tests/simulator --------------------------------- + def poke(self, addr: int, data: bytes) -> None: + off = addr - self.base + if off < 0 or off + len(data) > len(self.mem): + raise DebuggerError(f"address 0x{addr:08x} out of mock range") + self.mem[off:off + len(data)] = data + + # --- Debugger interface ------------------------------------------ + def flash(self, bin_path: str, load_addr: int) -> None: + self.flashed.append((bin_path, load_addr)) + + def read_memory(self, addr: int, length: int) -> bytes: + off = addr - self.base + if off < 0 or off + length > len(self.mem): + raise DebuggerError(f"read 0x{addr:08x}+{length} out of mock range") + return bytes(self.mem[off:off + length]) + + def write_u32(self, addr: int, value: int) -> None: + self.poke(addr, (value & 0xFFFFFFFF).to_bytes(4, "little")) diff --git a/hwci/hwci/debugger/openocd.py b/hwci/hwci/debugger/openocd.py new file mode 100644 index 000000000..7f2f4b966 --- /dev/null +++ b/hwci/hwci/debugger/openocd.py @@ -0,0 +1,210 @@ +"""OpenOCD (ST-Link) debugger backend. + +Drives a persistent ``openocd`` process and talks to its Tcl-RPC port (6666) to +read/write target memory while the firmware runs. Cortex-M memory access goes +through the AHB-AP and does not require halting the core, which is what makes +non-intrusive CPU-load/loop-time sampling possible (the same mechanism VS Code +"live watch" uses; note ``-gdb-max-connections`` in Mcu/f051/openocd.cfg). + +Flashing is done with a separate one-shot ``openocd`` invocation so the +persistent read session is never left in a halted state. + +This backend cannot be unit-tested without hardware; the offline test path uses +:class:`hwci.debugger.base.MockDebugger`. The Tcl-RPC framing and command set +here follow the documented OpenOCD interface. +""" +from __future__ import annotations + +import collections +import shutil +import socket +import struct +import subprocess +import threading +import time + +from .base import Debugger, DebuggerError + +# OpenOCD Tcl-RPC terminates every command and reply with this byte. +_RPC_SEP = b"\x1a" + +# Default config matching Mcu/f051/openocd.cfg (ST-Link + STM32F0 target). +DEFAULT_CONFIGS = ["interface/stlink.cfg", "target/stm32f0x.cfg"] +APP_LOAD_ADDR = 0x08001000 # AM32 app sits above the bootloader + + +class OpenOcdDebugger(Debugger): + def __init__( + self, + configs: list[str] | None = None, + *, + openocd_bin: str = "openocd", + tcl_port: int = 6666, + search_dirs: list[str] | None = None, + connect_timeout: float = 10.0, + ): + self.configs = configs or list(DEFAULT_CONFIGS) + self.openocd_bin = openocd_bin + self.tcl_port = tcl_port + self.search_dirs = search_dirs or [] + self.connect_timeout = connect_timeout + self._proc: subprocess.Popen | None = None + self._sock: socket.socket | None = None + self._log_tail: collections.deque[str] = collections.deque(maxlen=200) + self._drain_thread: threading.Thread | None = None + # The Tcl-RPC socket carries one command/reply at a time; the perf + # poller thread and the runner (stat resets at steady tails) both use + # it, so serialize access or the reply framing interleaves. + self._rpc_lock = threading.Lock() + if shutil.which(openocd_bin) is None: + raise DebuggerError(f"{openocd_bin!r} not found on PATH") + + # --- config helpers ---------------------------------------------- + def _base_args(self) -> list[str]: + args = [self.openocd_bin] + for d in self.search_dirs: + args += ["-s", d] + for c in self.configs: + args += ["-f", c] + return args + + # --- flashing (one-shot) ----------------------------------------- + def flash(self, bin_path: str, load_addr: int = APP_LOAD_ADDR) -> None: + cmd = self._base_args() + [ + "-c", f"program {{{bin_path}}} 0x{load_addr:08x} verify reset exit", + ] + proc = subprocess.run(cmd, capture_output=True, text=True, timeout=120) + if proc.returncode != 0: + raise DebuggerError( + f"flash failed (rc={proc.returncode}):\n{proc.stderr}\n{proc.stdout}") + + # --- persistent read session ------------------------------------- + def open(self) -> "OpenOcdDebugger": + """Start openocd (init, no halt) and connect to the Tcl-RPC port.""" + cmd = self._base_args() + [ + "-c", f"tcl_port {self.tcl_port}", + "-c", "gdb_port disabled", + "-c", "telnet_port disabled", + "-c", "init", + # leave the core running; only attach for background memory access + ] + self._proc = subprocess.Popen( + cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True) + # openocd logs continuously to stdout. NOBODY reading the pipe kills + # the session: once the 64 KiB pipe buffer fills, openocd blocks on + # write and every Tcl-RPC call times out from then on (observed on + # the bench as the perf channel dying at the same sample every run). + # Drain it forever; keep a tail for error diagnostics. + self._drain_thread = threading.Thread( + target=self._drain_stdout, daemon=True, name="openocd-drain") + self._drain_thread.start() + self._connect_rpc() + # Ensure the core is running (a fresh attach can leave it halted). + try: + self._rpc("resume") + except DebuggerError: + pass + return self + + def _drain_stdout(self) -> None: + proc = self._proc + if proc is None or proc.stdout is None: + return + try: + for line in proc.stdout: + self._log_tail.append(line.rstrip("\n")) + except (OSError, ValueError): + pass # pipe closed on shutdown + + def _connect_rpc(self) -> None: + deadline = time.monotonic() + self.connect_timeout + last_err: Exception | None = None + while time.monotonic() < deadline: + try: + self._sock = socket.create_connection( + ("127.0.0.1", self.tcl_port), timeout=2.0) + return + except OSError as e: # openocd not listening yet + last_err = e + if self._proc and self._proc.poll() is not None: + time.sleep(0.1) # let the drain thread catch the tail + out = "\n".join(self._log_tail) + raise DebuggerError(f"openocd exited early:\n{out}") + time.sleep(0.2) + raise DebuggerError(f"could not connect to openocd Tcl-RPC: {last_err}") + + def _rpc(self, command: str) -> str: + with self._rpc_lock: + return self._rpc_locked(command) + + def _rpc_locked(self, command: str) -> str: + if self._sock is None: + raise DebuggerError("RPC session not open; call open() first") + try: + self._sock.sendall(command.encode() + _RPC_SEP) + chunks = bytearray() + while _RPC_SEP not in chunks: + data = self._sock.recv(4096) + if not data: + raise DebuggerError("openocd RPC closed unexpectedly") + chunks += data + except (socket.timeout, OSError) as e: + # One glitched command desyncs the reply framing (the late reply + # would be read as the answer to the NEXT command), so a single + # hiccup would poison every subsequent read for the rest of the + # run. Rebuild the socket - openocd itself is still fine - and + # surface the error for this call only (observed on the bench: + # one SWD read timeout at motor spin-up killed the whole perf + # channel). + try: + self._sock.close() + except OSError: + pass + self._sock = None + self._connect_rpc() + raise DebuggerError(f"openocd RPC error for {command!r}: {e}") from e + return chunks.split(_RPC_SEP, 1)[0].decode(errors="replace") + + # --- Debugger interface ------------------------------------------ + def read_memory(self, addr: int, length: int) -> bytes: + nwords = (length + 3) // 4 + # read_memory returns space-separated decimal values (one per word). + out = self._rpc(f"read_memory 0x{addr:08x} 32 {nwords}") + tokens = out.replace("{", " ").replace("}", " ").split() + try: + words = [int(t, 0) for t in tokens] + except ValueError as e: + raise DebuggerError(f"unparseable read_memory reply {out!r}: {e}") + if len(words) < nwords: + raise DebuggerError( + f"short read: wanted {nwords} words, got {len(words)} ({out!r})") + return struct.pack(f"<{nwords}I", *words[:nwords])[:length] + + def write_u32(self, addr: int, value: int) -> None: + # OpenOCD reports failure IN-BAND: a successful mww replies with an + # empty string, a failed one with error text (no exception). Swallowing + # it would let e.g. a stats-reset silently no-op. + out = self._rpc(f"mww 0x{addr:08x} 0x{value & 0xFFFFFFFF:08x}") + if out.strip(): + raise DebuggerError(f"mww 0x{addr:08x} failed: {out.strip()!r}") + + def reset_run(self) -> None: + self._rpc("reset run") + + def close(self) -> None: + try: + if self._sock is not None: + try: + self._rpc("exit") + except DebuggerError: + pass + self._sock.close() + finally: + self._sock = None + if self._proc is not None: + self._proc.terminate() + try: + self._proc.wait(timeout=5) + except subprocess.TimeoutExpired: + self._proc.kill() + self._proc = None diff --git a/hwci/hwci/elf.py b/hwci/hwci/elf.py new file mode 100644 index 000000000..4dec00bfa --- /dev/null +++ b/hwci/hwci/elf.py @@ -0,0 +1,138 @@ +"""Locate the ``hwci_perf`` struct inside a built firmware ELF. + +The host never hard-codes a RAM address. It reads the address (and, when DWARF +is present, the full member layout) straight from the ELF that was flashed, so +the harness stays correct across firmware revisions and across the four +identical STM32F051 dies on the ARK 4IN1 (they share one image). + +Requires ``pyelftools``. +""" +from __future__ import annotations + +from dataclasses import dataclass + +try: + from elftools.elf.elffile import ELFFile + _HAVE_ELFTOOLS = True +except ImportError: # pragma: no cover - exercised only without the dep + _HAVE_ELFTOOLS = False + + +class ElfError(RuntimeError): + pass + + +class StructNotFoundError(ElfError): + """DWARF is present but the requested struct DIE is not. + + Distinct from "no DWARF at all": callers that soft-skip the layout + cross-check when debug info is stripped must still HARD-FAIL here - a + renamed/dropped struct is exactly the drift the check exists to catch.""" + + +def _require_elftools() -> None: + if not _HAVE_ELFTOOLS: + raise ElfError( + "pyelftools is required to read firmware symbols; " + "pip install pyelftools") + + +@dataclass +class Symbol: + name: str + address: int + size: int + + +def find_symbol(elf_path: str, name: str) -> Symbol: + """Return address+size of ``name`` from the ELF symbol table.""" + _require_elftools() + with open(elf_path, "rb") as fh: + elf = ELFFile(fh) + symtab = elf.get_section_by_name(".symtab") + if symtab is None: + raise ElfError(f"{elf_path}: no .symtab (build with debug symbols)") + matches = [s for s in symtab.iter_symbols() if s.name == name] + if not matches: + raise ElfError( + f"symbol {name!r} not found in {elf_path}; " + "is the firmware built with HWCI_PERF=1?") + sym = matches[0] + return Symbol(name=name, + address=sym["st_value"], + size=sym["st_size"]) + + +# Map (DWARF encoding, byte_size) -> struct code, matching perf.FIELDS codes. +_ENCODING_SIGNED = {5, 6, 0xd} # DW_ATE_signed / signed_char / (vendor) + + +@dataclass +class Member: + name: str + offset: int + size: int + signed: bool + + +def struct_layout(elf_path: str, type_name: str) -> list[Member]: + """Read the member layout of ``struct `` from DWARF debug info. + + Returns members in offset order. Raises :class:`ElfError` if the ELF has no + DWARF (firmware must be compiled with -g, which AM32 does by default). + """ + _require_elftools() + with open(elf_path, "rb") as fh: + elf = ELFFile(fh) + if not elf.has_dwarf_info(): + raise ElfError(f"{elf_path}: no DWARF info") + dwarf = elf.get_dwarf_info() + for cu in dwarf.iter_CUs(): + for die in cu.iter_DIEs(): + if die.tag != "DW_TAG_structure_type": + continue + name_attr = die.attributes.get("DW_AT_name") + if name_attr is None: + continue + if name_attr.value.decode("utf-8", "replace") != type_name: + continue + return _members_of(die, cu) + raise StructNotFoundError( + f"struct {type_name!r} not found in DWARF of {elf_path}") + + +def _members_of(struct_die, cu) -> list[Member]: + members: list[Member] = [] + for child in struct_die.iter_children(): + if child.tag != "DW_TAG_member": + continue + name = child.attributes["DW_AT_name"].value.decode("utf-8", "replace") + offset = child.attributes.get("DW_AT_data_member_location") + offset_val = offset.value if offset is not None else 0 + if isinstance(offset_val, list): # location expression form + # DW_OP_plus_uconst => [0x23, n] + offset_val = offset_val[1] if len(offset_val) > 1 else 0 + size, signed = _base_type(child, cu) + members.append(Member(name=name, offset=offset_val, + size=size, signed=signed)) + members.sort(key=lambda m: m.offset) + return members + + +def _base_type(member_die, cu) -> tuple[int, bool]: + type_ref = member_die.attributes.get("DW_AT_type") + if type_ref is None: + return 0, False + die = cu.get_DIE_from_refaddr(type_ref.value + cu.cu_offset) + # Walk through typedef/const/volatile qualifiers to the base type. + while die.tag in ("DW_TAG_typedef", "DW_TAG_const_type", + "DW_TAG_volatile_type"): + nxt = die.attributes.get("DW_AT_type") + if nxt is None: + return 0, False + die = cu.get_DIE_from_refaddr(nxt.value + cu.cu_offset) + size_attr = die.attributes.get("DW_AT_byte_size") + size = size_attr.value if size_attr is not None else 0 + enc_attr = die.attributes.get("DW_AT_encoding") + signed = bool(enc_attr and enc_attr.value in _ENCODING_SIGNED) + return size, signed diff --git a/hwci/hwci/esc_telem/__init__.py b/hwci/hwci/esc_telem/__init__.py new file mode 100644 index 000000000..8aa4e7c2c --- /dev/null +++ b/hwci/hwci/esc_telem/__init__.py @@ -0,0 +1,2 @@ +"""ESC-side telemetry decoding (KISS serial).""" +from .kiss import KissFrame, KissStream, crc8, encode_frame, parse_frame # noqa: F401 diff --git a/hwci/hwci/esc_telem/kiss.py b/hwci/hwci/esc_telem/kiss.py new file mode 100644 index 000000000..8667d2ff5 --- /dev/null +++ b/hwci/hwci/esc_telem/kiss.py @@ -0,0 +1,119 @@ +"""KISS / BLHeli ESC serial telemetry decoding. + +AM32 emits a 10-byte KISS telemetry frame on the dedicated telemetry wire +(``USE_SERIAL_TELEMETRY`` is enabled on the ARK 4IN1 target). The frame is the +struct ``kiss_telem_pkt_t`` in ``Inc/kiss_telemetry.h``: + + byte 0 int8 temperature, degrees C + byte 1..2 u16 BE voltage, centivolts + byte 3..4 u16 BE current, centiamps + byte 5..6 u16 BE consumption, mAh + byte 7..8 u16 BE eRPM / 100 + byte 9 u8 CRC-8 over bytes 0..8 + +The CRC-8 matches ``get_crc8``/``update_crc8`` in ``Src/functions.c`` +(polynomial 0x07, init 0x00). There is no start delimiter, so the streaming +parser resyncs by sliding a 10-byte window until the CRC validates. +""" +from __future__ import annotations + +import struct +from dataclasses import dataclass +from typing import Iterator + +FRAME_LEN = 10 + + +def _crc8_table() -> bytes: + table = bytearray(256) + for byte in range(256): + crc = byte + for _ in range(8): + crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF + table[byte] = crc + return bytes(table) + + +_CRC8_TABLE = _crc8_table() + + +def crc8(data: bytes) -> int: + """BLHeli/KISS CRC-8 (poly 0x07, init 0x00), matching Src/functions.c. + + Table-driven: the streaming framer runs this once per byte-slide when + resynchronising on a noisy line, so the bitwise loop would burn a core. + """ + crc = 0 + for byte in data: + crc = _CRC8_TABLE[crc ^ byte] + return crc + + +@dataclass +class KissFrame: + temperature_c: int + voltage_v: float + current_a: float + consumption_mah: int + e_rpm: int + crc_ok: bool + + def mech_rpm(self, pole_pairs: int) -> float: + return self.e_rpm / pole_pairs if pole_pairs else 0.0 + + +def parse_frame(data: bytes) -> KissFrame: + """Parse exactly one 10-byte frame. ``crc_ok`` flags CRC validity.""" + if len(data) < FRAME_LEN: + raise ValueError(f"need {FRAME_LEN} bytes, got {len(data)}") + temp, volt_cv, cur_ca, cons, erpm100, crc = struct.unpack( + ">bHHHHB", data[:FRAME_LEN]) + return KissFrame( + temperature_c=temp, + voltage_v=volt_cv / 100.0, + current_a=cur_ca / 100.0, + consumption_mah=cons, + e_rpm=erpm100 * 100, + crc_ok=(crc == crc8(data[:FRAME_LEN - 1])), + ) + + +def encode_frame(*, temperature_c: int, voltage_cv: int, current_ca: int, + consumption_mah: int, erpm100: int) -> bytes: + """Build one CRC'd 10-byte frame (inverse of :func:`parse_frame`). + + The single owner of the wire layout for producers (simulator, tests) so + an encoder can never drift from the parser.""" + body = struct.pack(">bHHHH", + max(-128, min(127, temperature_c)), + voltage_cv & 0xFFFF, current_ca & 0xFFFF, + consumption_mah & 0xFFFF, erpm100 & 0xFFFF) + return body + bytes([crc8(body)]) + + +class KissStream: + """Incremental framer for the KISS byte stream. + + Feed raw bytes from the serial port with :meth:`feed`; it yields every + CRC-valid frame found. Because the protocol is delimiter-less, a window that + fails CRC is advanced by a single byte to resynchronise. + """ + + def __init__(self) -> None: + self._buf = bytearray() + + def feed(self, data: bytes) -> Iterator[KissFrame]: + self._buf.extend(data) + while len(self._buf) >= FRAME_LEN: + window = bytes(self._buf[:FRAME_LEN]) + # CRC first: on a misaligned/noisy line the framer slides one byte + # at a time, and unpacking + constructing a KissFrame per slide + # would dominate the telemetry thread. + if crc8(window[:FRAME_LEN - 1]) == window[FRAME_LEN - 1]: + del self._buf[:FRAME_LEN] + yield parse_frame(window) + else: + del self._buf[:1] # resync + + def reset(self) -> None: + self._buf.clear() diff --git a/hwci/hwci/flightstand/__init__.py b/hwci/hwci/flightstand/__init__.py new file mode 100644 index 000000000..82059de31 --- /dev/null +++ b/hwci/hwci/flightstand/__init__.py @@ -0,0 +1,11 @@ +"""Flight Stand backends.""" +from .base import SafetyLimits, StandSafetyTripped, StandSample, ThrustStand # noqa: F401 +from .grpc_client import FlightStandGrpc, SignalMap # noqa: F401 + +# Lazy: simulator imports RigSimulator from hwci.sim, which imports StandSample +# from this package — a top-level SimulatedStand import would circular-import. +def __getattr__(name: str): + if name == "SimulatedStand": + from .simulator import SimulatedStand + return SimulatedStand + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") diff --git a/hwci/hwci/flightstand/base.py b/hwci/hwci/flightstand/base.py new file mode 100644 index 000000000..c6b62b774 --- /dev/null +++ b/hwci/hwci/flightstand/base.py @@ -0,0 +1,127 @@ +"""Thrust-stand backend abstraction. + +A :class:`ThrustStand` represents a Tyto Robotics Flight Stand (50, or any of +the family) and provides throttle control + synchronized force/electrical +measurement. The real backend talks gRPC to the Flight Stand Software; the +simulator implements the same interface for offline development and tests. + +Units are SI internally (Newtons, N*m, RPM, V, A). ``grams-force`` and +``g/W`` (the usual drone efficiency figure) are derived properties. +""" +from __future__ import annotations + +import abc +from dataclasses import dataclass + +G0 = 9.80665 # standard gravity, m/s^2 + + +class StandSafetyTripped(RuntimeError): + """A profile safety limit was breached; the run must abort immediately.""" + + +@dataclass +class StandSample: + t: float # host monotonic timestamp, seconds + throttle: float # last commanded throttle, 0..1 (echoed) + thrust_n: float + torque_nm: float + rpm: float # mechanical RPM + voltage_v: float + current_a: float + motor_temp_c: float | None = None # optional probe (None = not fitted) + fet_temp_c: float | None = None + + @property + def elec_power_w(self) -> float: + return self.voltage_v * self.current_a + + @property + def mech_power_w(self) -> float: + omega = self.rpm * 2.0 * 3.141592653589793 / 60.0 + return self.torque_nm * omega + + @property + def thrust_gf(self) -> float: + return self.thrust_n / G0 * 1000.0 + + @property + def efficiency_gf_per_w(self) -> float: + p = self.elec_power_w + return self.thrust_gf / p if p > 1e-6 else 0.0 + + @property + def motor_efficiency(self) -> float: + p = self.elec_power_w + return self.mech_power_w / p if p > 1e-6 else 0.0 + + +@dataclass +class SafetyLimits: + """Cutoffs enforced host-side by the runner on every sample (see + :func:`hwci.runner.enforce_safety`); a breach aborts the test immediately. + Backends may ALSO enforce them (the simulator does; the vendor Flight + Stand Software has its own UI cutoffs), but the runner check is the one + that is guaranteed to exist on every rig.""" + max_thrust_n: float | None = None + max_current_a: float | None = None + max_rpm: float | None = None + max_voltage_v: float | None = None + max_motor_temp_c: float | None = None + + def check(self, *, thrust_n: float | None = None, + current_a: float | None = None, rpm: float | None = None, + voltage_v: float | None = None, + temp_c: float | None = None) -> None: + """Raise :class:`StandSafetyTripped` if any provided value exceeds + its limit. ``None`` values (channel not available) are skipped.""" + def _over(value, limit): + return value is not None and limit is not None and value > limit + if _over(thrust_n, self.max_thrust_n): + raise StandSafetyTripped( + f"thrust {thrust_n:.2f} N > limit {self.max_thrust_n:.2f} N") + if _over(current_a, self.max_current_a): + raise StandSafetyTripped( + f"current {current_a:.1f} A > limit {self.max_current_a:.1f} A") + if _over(rpm, self.max_rpm): + raise StandSafetyTripped( + f"rpm {rpm:.0f} > limit {self.max_rpm:.0f}") + if _over(voltage_v, self.max_voltage_v): + raise StandSafetyTripped( + f"voltage {voltage_v:.2f} V > limit {self.max_voltage_v:.2f} V") + if _over(temp_c, self.max_motor_temp_c): + raise StandSafetyTripped( + f"temp {temp_c:.0f} C > limit {self.max_motor_temp_c:.0f} C") + + +class ThrustStand(abc.ABC): + @abc.abstractmethod + def open(self) -> "ThrustStand": + ... + + @abc.abstractmethod + def set_throttle(self, throttle: float) -> None: + """Command throttle in [0, 1].""" + + @abc.abstractmethod + def read_sample(self) -> StandSample: + ... + + def set_safety_limits(self, limits: SafetyLimits) -> None: # optional + pass + + def deactivate(self) -> None: # optional: drop the ESC signal (line low) + pass + + def tare(self) -> None: # optional: zero the load cells + pass + + def close(self) -> None: # optional + pass + + def __enter__(self) -> "ThrustStand": + return self.open() + + def __exit__(self, *exc) -> None: + self.set_throttle(0.0) + self.close() diff --git a/hwci/hwci/flightstand/grpc_client.py b/hwci/hwci/flightstand/grpc_client.py new file mode 100644 index 000000000..6f2604385 --- /dev/null +++ b/hwci/hwci/flightstand/grpc_client.py @@ -0,0 +1,326 @@ +"""Real Flight Stand gRPC backend (Tyto Robotics Flight Stand Software API v1). + +Mapped against ``flight_stand_api_v1.proto`` from +https://gitlab.com/TytoRobotics/flightstand-api (the pre-compiled Python stubs +in ``languages/python`` of that repo must be importable, e.g. via a ``.pth`` +file in the venv). + +The Tyto model: *boards* (USB measurement units) expose *inputs* (sensors, +identified by ``InputType``: FORCE_FZ=11 is thrust) and *outputs* (the ESC +signal, ``OutputType.ESC``). The latest value of every signal comes back in +one ``ListSamples`` round trip; throttle is commanded with ``UpdateOutput`` +on the output's ``output_target`` field (a µs-style value, 1000-2000 for +standard PWM). + +The Flight Stand Software runs on Windows only. For a Linux bench, run it on +a Windows PC on the same network, launched with ``--remote``, and point +``stand_host`` at that PC. Units over the API are SI: thrust in Newtons, +rotation speed in rad/s (converted to RPM here). + +The numeric ids in :class:`SignalMap` ARE Tyto ``InputType`` enum values, so +a rig file maps channels without touching code; ``esc_output`` indexes the +ESC-type outputs sorted by resource name. +""" +from __future__ import annotations + +import importlib +import math +import time + +from dataclasses import dataclass + +from .base import SafetyLimits, StandSample, ThrustStand + + +@dataclass +class SignalMap: + """Maps StandSample fields to Tyto ``InputType`` enum values. + + Defaults follow the proto: FORCE_FZ=11 thrust, TORQUE_MZ=14, ROTATION_ + SPEED_FREQUENCY=15 (rad/s), VOLTAGE_HV_INPUT=6, CURRENT_HALL_CURRENT=8. + Set a channel to ``null``/None in the rig file if the bench lacks it; + reads then report 0.0 and the metrics coverage gate flags it. + """ + thrust: int = 11 + torque: int | None = 14 + rpm: int | None = 15 + voltage: int | None = 6 + current: int | None = 8 + # Optional temperature channels. Values may be an InputType enum int, or + # a string: an input resource name ("/boards/COM3/inputs/29", stable per + # board) or a raw signal name ("/signals/37", renumbers on reconnect - + # prefer the input name). + motor_temp: int | str | None = None + fet_temp: int | str | None = None + # Output (actuator) used to command throttle, and its raw range. + esc_output: int = 0 + esc_min: float = 1000.0 # raw value of the LOWEST real throttle step + esc_max: float = 2000.0 # raw value of full throttle + # Raw value for throttle == 0. Leave None where zero throttle IS esc_min + # (standard PWM: 1000 us). For DShot it must be 0: AM32 only arms on + # sustained DShot 0 (what an FC sends while disarmed), values 1-47 are + # DShot COMMANDS that must never be emitted (a ramp sweeping them could + # trigger beacons or even settings changes), and real throttle starts at + # 48. So DShot: esc_zero=0, esc_min=48, esc_max=2047. + esc_zero: float | None = None + thrust_is_grams: bool = False # gRPC API is SI: Newtons + rpm_is_rad_per_s: bool = True # gRPC API reports rotation in rad/s + + +G0 = 9.80665 +RADS_TO_RPM = 60.0 / (2.0 * math.pi) + + +def _k_to_c(kelvin: float | None) -> float | None: + return kelvin - 273.15 if kelvin is not None else None + + +class _StubAdapter: + """The only proto-aware surface, mapped to Tyto's FlightStand service.""" + + RPC_TIMEOUT_S = 2.0 + + def __init__(self, host: str, port: int, pb2: str, pb2_grpc: str): + try: + self._pb2 = importlib.import_module(pb2) + self._pb2_grpc = importlib.import_module(pb2_grpc) + from google.protobuf import field_mask_pb2 + import grpc + except ImportError as e: + raise RuntimeError( + "Flight Stand gRPC stubs / grpcio not importable " + f"({e}). Clone https://gitlab.com/TytoRobotics/flightstand-api " + "and make languages/python importable (pip install grpcio " + "protobuf, then add a .pth file pointing at it).") from e + self._grpc = grpc + self._field_mask_pb2 = field_mask_pb2 + self._channel = grpc.insecure_channel(f"{host}:{port}") + self._stub = self._make_stub() + self._sig_by_type: dict[int, str] = {} + self._esc_outputs: list = [] + # Fail fast with an actionable message if the core is not running. + try: + self._stub.GetServerStatus(self._pb2.GetServerStatusRequest(), + timeout=self.RPC_TIMEOUT_S) + except grpc.RpcError as e: + raise RuntimeError( + f"Flight Stand core not reachable at {host}:{port} " + f"({e.code().name}). Start the Flight Stand Software " + "(with --remote if it runs on another machine).") from e + # A cutoff latched from a previous session blocks output commands. + try: + self._stub.ClearCutoff(self._pb2.ClearCutoffRequest(), + timeout=self.RPC_TIMEOUT_S) + except grpc.RpcError: + pass # no cutoff to clear + + # --- proto-specific operations ---------------------------------------- + def _make_stub(self): + # The generated grpc module exposes one Stub class. + stub_classes = [v for k, v in vars(self._pb2_grpc).items() + if k.endswith("Stub")] + if not stub_classes: + raise RuntimeError("no *Stub class found in grpc stubs module") + return stub_classes[0](self._channel) + + def list_boards(self): + """Boards that are connected and ready.""" + resp = self._stub.ListBoards(self._pb2.ListBoardsRequest(), + timeout=self.RPC_TIMEOUT_S) + boards = [b for b in resp.boards if b.ready] + if not boards: + raise RuntimeError( + "Flight Stand core is running but reports no ready boards - " + "check the stand's USB connections on the machine running " + "the Flight Stand Software.") + return boards + + def connect_board(self, board) -> None: + """USB boards auto-connect; discover the signal/output mapping. + + Inputs and outputs are discovered across ALL ready boards (the + Flight Stand 50 enumerates as separate force and power units), so + ``board`` only anchors the error message. + """ + ins = self._stub.ListInputs(self._pb2.ListInputsRequest(), + timeout=self.RPC_TIMEOUT_S).inputs + self._sig_by_type = {} + self._sig_by_input_name = {} + for i in ins: + # First input of each type wins, matching the vendor helper's + # find_input_by_type(). + self._sig_by_type.setdefault(int(i.input_type), i.signal_name) + self._sig_by_input_name[i.name] = i.signal_name + + outs = self._stub.ListOutputs(self._pb2.ListOutputsRequest(), + timeout=self.RPC_TIMEOUT_S).outputs + self._esc_outputs = sorted( + (o for o in outs if o.output_type == self._pb2.ESC and not o.closed), + key=lambda o: o.name) + if not self._esc_outputs: + raise RuntimeError( + f"no ESC output found on {board.name} (or any board) - " + "cannot command throttle") + + def input_types_available(self) -> dict[int, str]: + """InputType -> signal_name map discovered at connect (diagnostics).""" + return dict(self._sig_by_type) + + def _resolve(self, sid) -> str | None: + """Channel id -> signal name. int = InputType enum; str = input + resource name (preferred, stable) or raw /signals/N name.""" + if isinstance(sid, str): + if sid.startswith("/signals/"): + return sid + return self._sig_by_input_name.get(sid) + return self._sig_by_type.get(sid) + + def read_signal(self, signal_id) -> float: + return self.read_signals([signal_id]).get(signal_id, 0.0) + + def read_signals(self, signal_ids: list) -> dict: + """Latest value for each wanted channel in ONE round trip. + + ``ListSamples`` returns the most recent sample of every signal; + inactive samples (sensor off/disconnected) are omitted so the + caller's 0.0 default and the coverage gate see a dead channel. + """ + wanted = {} + for sid in signal_ids: + name = self._resolve(sid) + if name is not None: + wanted[name] = sid + if not wanted: + return {} + resp = self._stub.ListSamples(self._pb2.ListSamplesRequest(), + timeout=self.RPC_TIMEOUT_S) + out: dict[int, float] = {} + for sample in resp.sample_group.samples: + t = wanted.get(sample.signal_name) + if t is not None and sample.active: + out[t] = sample.value + return out + + def set_output(self, output_id: int, value: float, *, + active: bool = True) -> None: + try: + output = self._esc_outputs[output_id] + except IndexError: + raise RuntimeError( + f"esc_output index {output_id} out of range - only " + f"{len(self._esc_outputs)} ESC output(s) discovered") + target = self._pb2.OutputTarget(active=active, target_value=value, + rate_limit_per_second=0.0) + mask = self._field_mask_pb2.FieldMask(paths=["output_target"]) + req = self._pb2.UpdateOutputRequest( + output=self._pb2.Output(name=output.name, output_target=target), + mask=mask) + self._stub.UpdateOutput(req, timeout=self.RPC_TIMEOUT_S) + + def tare(self) -> None: + self._stub.TareInputs(self._pb2.TareInputsRequest(), + timeout=30.0) # taring takes a few seconds + + def close_channel(self) -> None: + self._channel.close() + + +class FlightStandGrpc(ThrustStand): + def __init__( + self, + host: str = "127.0.0.1", + port: int = 50051, + *, + signals: SignalMap | None = None, + pb2_module: str = "flight_stand_api_v1_pb2", + pb2_grpc_module: str = "flight_stand_api_v1_pb2_grpc", + board_index: int = 0, + ): + self.host = host + self.port = port + self.signals = signals or SignalMap() + self._pb2_module = pb2_module + self._pb2_grpc_module = pb2_grpc_module + self._board_index = board_index + self._api: _StubAdapter | None = None + self._throttle = 0.0 + + def open(self) -> "FlightStandGrpc": + self._api = _StubAdapter(self.host, self.port, + self._pb2_module, self._pb2_grpc_module) + boards = self._api.list_boards() + self._api.connect_board(boards[self._board_index]) + return self + + def set_throttle(self, throttle: float) -> None: + throttle = max(0.0, min(1.0, throttle)) + self._throttle = throttle + s = self.signals + if throttle <= 0.0 and s.esc_zero is not None: + raw = s.esc_zero # e.g. DShot 0: disarm-idle/motor stop + else: + raw = s.esc_min + throttle * (s.esc_max - s.esc_min) + self._api.set_output(s.esc_output, raw) + + def read_sample(self) -> StandSample: + s = self.signals + wanted = [sid for sid in (s.thrust, s.torque, s.rpm, s.voltage, + s.current, s.motor_temp, s.fet_temp) + if sid is not None] + values = self._api.read_signals(wanted) if wanted else {} + + def _get(signal_id) -> float: + return values.get(signal_id, 0.0) if signal_id is not None else 0.0 + + thrust = _get(s.thrust) + thrust_n = thrust * G0 / 1000.0 if s.thrust_is_grams else thrust + rpm = _get(s.rpm) + if s.rpm_is_rad_per_s: + rpm *= RADS_TO_RPM + return StandSample( + t=time.monotonic(), + throttle=self._throttle, + thrust_n=thrust_n, + torque_nm=_get(s.torque), + rpm=rpm, + voltage_v=_get(s.voltage), + current_a=_get(s.current), + # None (not 0.0) when unmapped/inactive: a dead temp probe must + # read as missing, not as a freezing motor. The API is strict SI, + # so temperatures arrive in Kelvin. + motor_temp_c=_k_to_c(values.get(s.motor_temp)) if s.motor_temp is not None else None, + fet_temp_c=_k_to_c(values.get(s.fet_temp)) if s.fet_temp is not None else None, + ) + + def set_safety_limits(self, limits: SafetyLimits) -> None: + # NOTE: this backend cannot enforce limits itself until the vendor + # cutoff-condition RPC is mapped in _StubAdapter. Enforcement happens + # in the runner (hwci.runner.enforce_safety) against every sample; + # configure the Flight Stand Software's own UI cutoffs as a second + # layer. + self._limits = limits + + def deactivate(self) -> None: + """Drop the ESC signal entirely (output inactive drives logic 0).""" + self._api.set_output(self.signals.esc_output, self.signals.esc_min, + active=False) + + def tare(self) -> None: + if self._api is not None: + self._api.tare() + + def close(self) -> None: + if self._api is not None: + park = (self.signals.esc_zero if self.signals.esc_zero is not None + else self.signals.esc_min) + try: + # Park at zero throttle, then drop the signal entirely. + self._api.set_output(self.signals.esc_output, park) + self._api.set_output(self.signals.esc_output, park, + active=False) + except Exception: + pass + try: + self._api.close_channel() + except Exception: + pass diff --git a/hwci/hwci/flightstand/simulator.py b/hwci/hwci/flightstand/simulator.py new file mode 100644 index 000000000..b27e6b2b0 --- /dev/null +++ b/hwci/hwci/flightstand/simulator.py @@ -0,0 +1,62 @@ +"""Simulated Flight Stand backed by :class:`hwci.sim.RigSimulator`. + +Lets the entire harness run end-to-end with no hardware. The same +:class:`~hwci.sim.RigSimulator` instance also feeds the simulated KISS telemetry +and perf-struct sources so all three channels stay consistent (see +:func:`hwci.runner.build_sim_sources`). +""" +from __future__ import annotations + +import time +from typing import Callable + +from ..sim import MotorParams, RigSimulator +from .base import SafetyLimits, StandSafetyTripped, StandSample, ThrustStand + +__all__ = ["SimulatedStand", "StandSafetyTripped"] + + +class SimulatedStand(ThrustStand): + def __init__( + self, + rig: RigSimulator | None = None, + *, + clock: Callable[[], float] = time.monotonic, + fixed_dt: float | None = None, + params: MotorParams | None = None, + ): + self.rig = rig or RigSimulator(params=params or MotorParams()) + self._clock = clock + self._fixed_dt = fixed_dt + self._throttle = 0.0 + self._last_t: float | None = None + self._limits = SafetyLimits() + + def open(self) -> "SimulatedStand": + self._last_t = self._clock() + return self + + def set_throttle(self, throttle: float) -> None: + self._throttle = max(0.0, min(1.0, throttle)) + + def set_safety_limits(self, limits: SafetyLimits) -> None: + self._limits = limits + + def read_sample(self) -> StandSample: + now = self._clock() + if self._fixed_dt is not None: + dt = self._fixed_dt + else: + dt = 0.0 if self._last_t is None else max(0.0, now - self._last_t) + self._last_t = now + self.rig.step(dt, self._throttle) + sample = self.rig.stand_sample(now) + self._check_limits(sample) + return sample + + def _check_limits(self, s: StandSample) -> None: + self._limits.check(thrust_n=s.thrust_n, current_a=s.current_a, + rpm=s.rpm, voltage_v=s.voltage_v) + + def close(self) -> None: + self._throttle = 0.0 diff --git a/hwci/hwci/metrics.py b/hwci/hwci/metrics.py new file mode 100644 index 000000000..a3b04adaa --- /dev/null +++ b/hwci/hwci/metrics.py @@ -0,0 +1,386 @@ +"""Compute performance metrics from a run. + +Produces three things the baseline/report care about: + +* steady-state operating points (thrust, power, efficiency g/W, plus loop time + and CPU load at each throttle), +* worst-case loop timing and CPU load over the whole run, +* host-side demag / desync detection (firmware bemf-timeout flag, commutation- + interval spikes, RPM collapse, and ESC-eRPM vs stand-RPM divergence). + +CPU load uses the idle-residual method: the firmware exposes a free-running +``loop_iters`` counter; its rate (iters/s) is highest when the core is least +loaded, so ``cpu_load = 1 - rate/idle_rate`` where ``idle_rate`` is the rate +measured at zero throttle. This is how CPU load is recovered on a Cortex-M0, +which has no cycle counter. +""" +from __future__ import annotations + +import numpy as np + +from .config import Profile +from .model import RunResult + + +def _col(rows: list[dict], name: str) -> np.ndarray: + out = np.full(len(rows), np.nan) + for i, r in enumerate(rows): + v = r.get(name) + if v is not None and v != "": + try: + out[i] = float(v) + except (TypeError, ValueError): + pass + return out + + +def _loop_iter_rate(t: np.ndarray, iters: np.ndarray) -> np.ndarray: + """Per-sample loop-iteration rate (Hz), NaN where undefined.""" + rate = np.full(len(t), np.nan) + dt = np.diff(t) + di = np.diff(iters) + with np.errstate(invalid="ignore", divide="ignore"): + r = np.where((dt > 0) & (di >= 0), di / dt, np.nan) + rate[1:] = r + return rate + + +def _cpu_load(rows: list[dict]) -> tuple[np.ndarray, float]: + # Prefer the timestamp taken at the actual SWD read (perf_host_t) over the + # sample-loop schedule time: a host stall between ticks would otherwise + # attribute too many loop iterations to too little time and corrupt the + # rate for that sample. + t_host = _col(rows, "perf_host_t") + t = t_host if not np.all(np.isnan(t_host)) else _col(rows, "t") + iters = _col(rows, "perf_loop_iters") + throttle = _col(rows, "throttle_cmd") + running = _col(rows, "perf_running") + rate = _loop_iter_rate(t, iters) + # Median, not max: one glitched sample must not become the reference the + # whole run's load is scaled by. + # + # The reference must be "motor genuinely stopped" (firmware-reported + # perf_running == 0), NOT "commanded throttle below a small cutoff": a + # segment that RAMPS UP FROM ZERO (e.g. efficiency_sweep's t10, 0%->10%) + # commands throttle under any such cutoff for real, non-idle time while + # the motor is actively spinning up and commutating - a genuine load, not + # idle. Observed on the bench: lengthening t10's hold pushed these + # ramp-up samples from ~37% to ~50% of the throttle-based reference pool, + # collapsing the reported idle rate from ~60k to ~46k iters/s and + # understating max_cpu_load_pct by 10+ points. perf_running falls back to + # the throttle heuristic only for run data recorded before that column + # existed. + if not np.all(np.isnan(running)): + idle_mask = (running == 0) & ~np.isnan(rate) + else: + idle_mask = (throttle < 0.02) & ~np.isnan(rate) + if idle_mask.any(): + idle_rate = float(np.nanmedian(rate[idle_mask])) + elif not np.all(np.isnan(rate)): + idle_rate = float(np.nanmedian(rate)) + else: + return np.full(len(rows), np.nan), float("nan") + if idle_rate <= 0: + return np.full(len(rows), np.nan), idle_rate + load = 100.0 * (1.0 - rate / idle_rate) + return np.clip(load, 0.0, 100.0), idle_rate + + +def tail_start_index(n: int, fraction: float) -> int: + """First index of the trailing ``fraction`` of ``n`` samples. + + The single source of truth for "where does the steady tail begin" - + hwci.runner's mid-segment perf-stats reset computes its own tick relative + to THIS function (with a safety margin) specifically so the two can never + drift apart. They used to be two independently-rounded formulas that + disagreed by a tick in some segment lengths, which is exactly the kind of + gap a reset-propagation race can hide in. + """ + if n <= 0: + return 0 + return int(n * (1.0 - fraction)) + + +def _tail(idx: np.ndarray, fraction: float) -> np.ndarray: + if len(idx) == 0: + return idx + return idx[tail_start_index(len(idx), fraction):] + + +def _nanmean(a: np.ndarray) -> float: + return float(np.nanmean(a)) if a.size and not np.all(np.isnan(a)) else float("nan") + + +_U32 = 1 << 32 + + +def _wrap32(a: float, b: float) -> int: + """b - a for firmware u32 counters, correct across a single wraparound.""" + return (int(b) - int(a)) % _U32 + + +def zc_jitter_window(count: np.ndarray, jsum: np.ndarray, isum: np.ndarray, + jmax: np.ndarray, idx: np.ndarray) -> dict: + """Zero-cross jitter over the sample window ``idx``. + + The firmware accumulates |interval deviation| and raw interval per + commutation into monotonic u32 sums (struct v2, see HWCI_PERF_ZC in + Inc/hwci_perf.h), so differencing the first and last valid snapshot in the + window yields per-commutation means with EVERY commutation counted - + immune to the ~200 Hz host sampling rate, which is far below the multi-kHz + commutation rate. Returns: + + * ``mean_pct`` - mean |deviation| as % of mean interval (the headline + zero-cross detection noise figure), + * ``max_pct`` - worst single deviation (sticky firmware max, reset by + the runner at each steady tail) as % of mean interval, + * both ``None`` when the firmware predates v2 or the window saw no + accumulated commutations (motor stopped / startup-gated). + """ + none = {"mean_pct": None, "max_pct": None} + if idx.size == 0: + return none + valid = idx[~np.isnan(count[idx]) & ~np.isnan(jsum[idx]) & ~np.isnan(isum[idx])] + if valid.size < 2: + return none + a, b = valid[0], valid[-1] + n = _wrap32(count[a], count[b]) + d_int = _wrap32(isum[a], isum[b]) + if n <= 0 or d_int <= 0: + return none + d_jit = _wrap32(jsum[a], jsum[b]) + mean_interval = d_int / n + out = {"mean_pct": round(100.0 * d_jit / d_int, 3), "max_pct": None} + m = jmax[valid] + if not np.all(np.isnan(m)): + out["max_pct"] = round(100.0 * float(np.nanmax(m)) / mean_interval, 2) + return out + + +def compute(run: RunResult, profile: Profile) -> dict: + rows = run.rows + seg = np.array([r.get("segment") for r in rows], dtype=object) + load, idle_rate = _cpu_load(rows) + + thrust_gf = _col(rows, "stand_thrust_gf") + current = _col(rows, "stand_current_a") + eff = _col(rows, "stand_eff_gf_per_w") + stand_rpm = _col(rows, "stand_rpm") + stand_v = _col(rows, "stand_voltage_v") + stand_pw = _col(rows, "stand_elec_power_w") + motor_temp = _col(rows, "stand_motor_temp_c") + fet_temp = _col(rows, "stand_fet_temp_c") + ctrl_exec = _col(rows, "perf_ctrl_exec_us_max") + ctrl_pmax = _col(rows, "perf_ctrl_period_us_max") + ctrl_pmin = _col(rows, "perf_ctrl_period_us_min") + main_max = _col(rows, "perf_main_loop_us_max") + perf_iters = _col(rows, "perf_loop_iters") + esc_erpm = _col(rows, "esc_erpm") + zc_count = _col(rows, "perf_zc_count") + zc_jsum = _col(rows, "perf_zc_jitter_sum") + zc_isum = _col(rows, "perf_zc_interval_sum") + zc_jmax = _col(rows, "perf_zc_jitter_max") + + steady_points = [] + for s in profile.segments: + if not s.steady: + continue + idx = np.where(seg == s.label)[0] + tail = _tail(idx, profile.steady_tail_fraction) + if tail.size == 0: + continue + jitter = zc_jitter_window(zc_count, zc_jsum, zc_isum, zc_jmax, tail) + steady_points.append({ + "segment": s.label, + "throttle": s.throttle, + "rpm": round(_nanmean(stand_rpm[tail]), 1), + "thrust_gf": round(_nanmean(thrust_gf[tail]), 2), + "current_a": round(_nanmean(current[tail]), 3), + "voltage_v": round(_nanmean(stand_v[tail]), 3), + "elec_power_w": round(_nanmean(stand_pw[tail]), 2), + "eff_gf_per_w": round(_nanmean(eff[tail]), 3), + "ctrl_exec_us_max": _safe_max(ctrl_exec[tail]), + "main_loop_us_max": _safe_max(main_max[tail]), + "cpu_load_pct": round(_nanmean(load[tail]), 1), + "motor_temp_c": round(_nanmean(motor_temp[tail]), 2), + "fet_temp_c": round(_nanmean(fet_temp[tail]), 2), + # zero-cross detection noise (report-only for now: gate once the + # bench has repeat captures establishing its run-to-run spread) + "zc_jitter_pct": jitter["mean_pct"], + "zc_jitter_max_pct": jitter["max_pct"], + }) + + demag = detect_demag(run, profile) + + summary = { + "max_thrust_gf": _safe_max(thrust_gf), + "max_current_a": round(_safe_maxf(current), 2), + "peak_efficiency_gf_per_w": round( + max((p["eff_gf_per_w"] for p in steady_points), default=float("nan")), 3), + "worst_ctrl_exec_us": _safe_max(ctrl_exec), + # Steady-window worst case: the runner resets the firmware's sticky + # accumulators at each steady tail, so these exclude motor start/stop + # transients (which vary 30%+ run-to-run) and are the values the + # baseline gate compares. + "worst_ctrl_exec_us_steady": max( + (p["ctrl_exec_us_max"] for p in steady_points + if p.get("ctrl_exec_us_max") is not None), default=None), + "worst_ctrl_period_us": _safe_max(ctrl_pmax), + "best_ctrl_period_us": _safe_minf(ctrl_pmin), + "worst_main_loop_us": _safe_max(main_max), + "worst_main_loop_us_steady": max( + (p["main_loop_us_max"] for p in steady_points + if p.get("main_loop_us_max") is not None), default=None), + "max_cpu_load_pct": round(_safe_maxf(load), 1), + "idle_loop_rate_hz": round(idle_rate, 1) if idle_rate == idle_rate else None, + # Worst steady-point zero-cross jitter (mean-of-window %, and worst + # single deviation %). Report-only: not compared by baseline.compare. + "worst_zc_jitter_pct": max( + (p["zc_jitter_pct"] for p in steady_points + if p.get("zc_jitter_pct") is not None), default=None), + "worst_zc_jitter_max_pct": max( + (p["zc_jitter_max_pct"] for p in steady_points + if p.get("zc_jitter_max_pct") is not None), default=None), + "demag_events": demag["event_count"], + "bemf_timeout_samples": demag["bemf_timeout_samples"], + "max_motor_temp_c": _safe_maxf(motor_temp), + "max_fet_temp_c": _safe_maxf(fet_temp), + # Channel liveness: how many samples each instrumentation channel + # actually delivered. The baseline gate fails a run whose coverage + # collapsed vs the baseline (dead SWD/telemetry/stand channel). + "n_samples": len(rows), + "perf_sample_count": int(np.count_nonzero(~np.isnan(perf_iters))), + "stand_sample_count": int(np.count_nonzero(~np.isnan(thrust_gf))), + "telem_sample_count": int(np.count_nonzero(~np.isnan(esc_erpm))), + } + return {"summary": summary, "steady_points": steady_points, "demag": demag} + + +def detect_demag(run: RunResult, profile: Profile) -> dict: + rows = run.rows + throttle = _col(rows, "throttle_cmd") + comm = _col(rows, "perf_commutation_interval") + bemf = _col(rows, "perf_bemf_timeout") + stand_rpm = _col(rows, "stand_rpm") + esc_erpm = _col(rows, "esc_erpm") + # Pole pairs are a property of the rig's motor; the run meta records the + # rig value at run time. profile.pole_pairs is only the offline fallback. + pp = int(run.meta.get("pole_pairs") or profile.pole_pairs) + + running = throttle > 0.2 + comm_running = comm[running & ~np.isnan(comm) & (comm > 0)] + median_comm = float(np.median(comm_running)) if comm_running.size else float("nan") + spike_thr = median_comm * profile.demag_commutation_spike if median_comm == median_comm else np.inf + + # per-sample anomaly flag + flag = np.zeros(len(rows), dtype=bool) + bemf_samples = 0 + spike_samples = 0 + for i in range(len(rows)): + if not running[i]: + continue + anom = False + # bemf_timeout_happened is an incrementing counter in firmware (1, 2, + # ... latched at 102 under stuck-rotor protection), not a boolean. + if bemf[i] >= 1: + bemf_samples += 1 + anom = True + if comm[i] == comm[i] and comm[i] > spike_thr: + spike_samples += 1 + anom = True + flag[i] = anom + + # Stand-RPM collapse while commanded throttle is high and NOT being + # intentionally reduced: catches desyncs whose transient firmware flags + # fall between SWD samples. The reference is the running max RPM seen + # since the throttle last decreased, so spool-up after a step never + # reads as a collapse. + # + # The gate must check the THROTTLE TREND (falling vs. not), not just the + # size of one tick's change: a smooth multi-second ramp-down moves by a + # tiny amount each 10ms tick, so a small-delta check like "< 0.02" never + # fires and the reference RPM is never invalidated - actual RPM then + # falls (correctly, because the ramp commanded it to) while the stale + # peak reference stays pinned at the ramp's starting RPM, eventually + # tripping the drop threshold. Observed on the bench: efficiency_sweep's + # 4s 100%->0% rampdn flagged a false demag event with zero bemf timeouts, + # commutation spikes, or eRPM/stand-RPM mismatch - RPM was tracking + # throttle exactly as commanded. + rpm_drop_samples = 0 + frac = profile.demag_rpm_drop_fraction + ref_rpm = float("nan") + for i in range(len(rows)): + high = throttle[i] == throttle[i] and throttle[i] > 0.5 + not_decreasing = (i > 0 and throttle[i - 1] == throttle[i - 1] + and throttle[i] >= throttle[i - 1] - 1e-6) + if not (high and not_decreasing): + ref_rpm = float("nan") + continue + r = stand_rpm[i] + if r != r: + continue + if ref_rpm != ref_rpm: + ref_rpm = r + continue + ref_rpm = max(ref_rpm, r) + if ref_rpm > 1000.0 and r < ref_rpm * (1.0 - frac): + rpm_drop_samples += 1 + flag[i] = True + + # debounce contiguous flagged samples into events (close gaps <= 3) + events = _events_from_flags(flag, max_gap=3) + + # ESC eRPM vs stand RPM divergence (telemetry desync indicator) + mismatch = 0 + for i in range(len(rows)): + if running[i] and esc_erpm[i] == esc_erpm[i] and stand_rpm[i] == stand_rpm[i] \ + and stand_rpm[i] > 500: + esc_mech = esc_erpm[i] / pp + if abs(esc_mech - stand_rpm[i]) / stand_rpm[i] > 0.2: + mismatch += 1 + + return { + "event_count": len(events), + "events": [{"start_idx": int(a), "end_idx": int(b)} for a, b in events], + "bemf_timeout_samples": bemf_samples, + "comm_spike_samples": spike_samples, + "rpm_drop_samples": rpm_drop_samples, + "median_commutation_interval": round(median_comm, 1) if median_comm == median_comm else None, + "esc_rpm_mismatch_samples": mismatch, + } + + +def _events_from_flags(flag: np.ndarray, max_gap: int = 3) -> list[tuple[int, int]]: + events: list[tuple[int, int]] = [] + start = None + gap = 0 + for i, f in enumerate(flag): + if f: + if start is None: + start = i + last = i + gap = 0 + elif start is not None: + gap += 1 + if gap > max_gap: + events.append((start, last)) + start = None + if start is not None: + events.append((start, last)) + return events + + +def _safe_max(a: np.ndarray): + if a.size and not np.all(np.isnan(a)): + return int(np.nanmax(a)) + return None + + +def _safe_maxf(a: np.ndarray) -> float: + return float(np.nanmax(a)) if a.size and not np.all(np.isnan(a)) else float("nan") + + +def _safe_minf(a: np.ndarray): + if a.size and not np.all(np.isnan(a)): + return int(np.nanmin(a)) + return None diff --git a/hwci/hwci/model.py b/hwci/hwci/model.py new file mode 100644 index 000000000..61e9c2520 --- /dev/null +++ b/hwci/hwci/model.py @@ -0,0 +1,149 @@ +"""Run data model: flat, columnar samples plus run metadata. + +A run is a list of flat row dicts (one per sample tick) over a fixed column set, +serialized as ``samples.csv`` next to a ``meta.json``. Keeping rows flat avoids +a pandas dependency and makes the metric math trivial with numpy. +""" +from __future__ import annotations + +import csv +import json +from dataclasses import dataclass, field +from pathlib import Path + +from .esc_telem.kiss import KissFrame +from .flightstand.base import StandSample +from .perf import PerfSample + +COLUMNS = [ + "t", "segment", "throttle_cmd", + # thrust stand + "stand_thrust_n", "stand_thrust_gf", "stand_torque_nm", "stand_rpm", + "stand_voltage_v", "stand_current_a", "stand_elec_power_w", + "stand_eff_gf_per_w", "stand_motor_temp_c", "stand_fet_temp_c", + # ESC KISS telemetry + "esc_erpm", "esc_voltage_v", "esc_current_a", "esc_temp_c", + # firmware perf struct (perf_host_t = host monotonic clock at the actual + # SWD read, so counter-rate math is immune to sample-loop scheduling jitter) + "perf_host_t", + "perf_ctrl_exec_us_last", "perf_ctrl_exec_us_max", + "perf_ctrl_period_us_max", "perf_ctrl_period_us_min", + "perf_main_loop_us_max", "perf_loop_iters", "perf_zero_cross_count", + "perf_commutation_interval", "perf_commutation_interval_max", + # zero-cross jitter accumulators (struct v2+; blank when the flashed + # firmware predates them - metrics treat blank as "metric unavailable") + "perf_zc_count", "perf_zc_jitter_sum", "perf_zc_interval_sum", + "perf_zc_jitter_max", + "perf_bemf_timeout", "perf_e_rpm", + # ESC input/arming state (proves the ESC decoded the throttle protocol) + "perf_input", "perf_armed", "perf_running", + # bidirectional DShot health (struct v3+; blank on older firmware) + "perf_dshot_rx_good", "perf_dshot_rx_bad", "perf_dshot_tx_frames", + "perf_dshot_last_com_us", "perf_dshot_telem_mode", "perf_dshot_edt_mode", +] + + +def make_row(t: float, segment: str, throttle_cmd: float, + stand: StandSample | None, + telem: KissFrame | None, + perf: PerfSample | None) -> dict: + row = {c: "" for c in COLUMNS} + row["t"] = round(t, 6) + row["segment"] = segment + row["throttle_cmd"] = round(throttle_cmd, 4) + if stand is not None: + row.update( + stand_thrust_n=round(stand.thrust_n, 5), + stand_thrust_gf=round(stand.thrust_gf, 3), + stand_torque_nm=round(stand.torque_nm, 6), + stand_rpm=round(stand.rpm, 1), + stand_voltage_v=round(stand.voltage_v, 3), + stand_current_a=round(stand.current_a, 3), + stand_elec_power_w=round(stand.elec_power_w, 3), + stand_eff_gf_per_w=round(stand.efficiency_gf_per_w, 4), + ) + if stand.motor_temp_c is not None: + row["stand_motor_temp_c"] = round(stand.motor_temp_c, 2) + if stand.fet_temp_c is not None: + row["stand_fet_temp_c"] = round(stand.fet_temp_c, 2) + if telem is not None: + row.update( + esc_erpm=telem.e_rpm, + esc_voltage_v=round(telem.voltage_v, 3), + esc_current_a=round(telem.current_a, 3), + esc_temp_c=telem.temperature_c, + ) + if perf is not None: + r = perf.raw + if perf.host_monotonic is not None: + row["perf_host_t"] = round(perf.host_monotonic, 6) + row.update( + perf_ctrl_exec_us_last=r["ctrl_exec_us_last"], + perf_ctrl_exec_us_max=r["ctrl_exec_us_max"], + perf_ctrl_period_us_max=r["ctrl_period_us_max"], + perf_ctrl_period_us_min=r["ctrl_period_us_min"], + perf_main_loop_us_max=r["main_loop_us_max"], + perf_loop_iters=r["loop_iters"], + perf_zero_cross_count=r["zero_cross_count"], + perf_commutation_interval=r["commutation_interval"], + perf_commutation_interval_max=r["commutation_interval_max"], + perf_bemf_timeout=r["bemf_timeout_state"], + perf_e_rpm=perf.e_rpm, + perf_input=r["input"], + perf_armed=r["armed"], + perf_running=r["running"], + ) + if "zc_count" in r: # struct v2+ + row.update( + perf_zc_count=r["zc_count"], + perf_zc_jitter_sum=r["zc_jitter_sum"], + perf_zc_interval_sum=r["zc_interval_sum"], + perf_zc_jitter_max=r["zc_jitter_max"], + ) + if "dshot_rx_good" in r: # struct v3+ + row.update( + perf_dshot_rx_good=r["dshot_rx_good"], + perf_dshot_rx_bad=r["dshot_rx_bad"], + perf_dshot_tx_frames=r["dshot_tx_frames"], + perf_dshot_last_com_us=r["dshot_last_com_us"], + perf_dshot_telem_mode=r["dshot_telem_mode"], + perf_dshot_edt_mode=r["dshot_edt_mode"], + ) + return row + + +@dataclass +class RunResult: + meta: dict = field(default_factory=dict) + rows: list[dict] = field(default_factory=list) + + def save(self, run_dir: str | Path) -> Path: + run_dir = Path(run_dir) + run_dir.mkdir(parents=True, exist_ok=True) + with open(run_dir / "samples.csv", "w", newline="") as fh: + writer = csv.DictWriter(fh, fieldnames=COLUMNS) + writer.writeheader() + writer.writerows(self.rows) + with open(run_dir / "meta.json", "w") as fh: + json.dump(self.meta, fh, indent=2, sort_keys=True) + return run_dir + + @classmethod + def load(cls, run_dir: str | Path) -> "RunResult": + run_dir = Path(run_dir) + meta = json.loads((run_dir / "meta.json").read_text()) + rows: list[dict] = [] + with open(run_dir / "samples.csv", newline="") as fh: + for raw in csv.DictReader(fh): + rows.append({k: _coerce(v) for k, v in raw.items()}) + return cls(meta=meta, rows=rows) + + +def _coerce(value: str): + if value == "" or value is None: + return None + try: + f = float(value) + return int(f) if f.is_integer() else f + except ValueError: + return value diff --git a/hwci/hwci/perf.py b/hwci/hwci/perf.py new file mode 100644 index 000000000..5a5949ce2 --- /dev/null +++ b/hwci/hwci/perf.py @@ -0,0 +1,219 @@ +"""Decoder for the firmware ``hwci_perf`` instrumentation struct. + +This mirrors, byte-for-byte, the C struct defined in ``Inc/hwci_perf.h``. The +canonical layouts below are the source of truth for offline use and tests; on +the real rig :mod:`hwci.elf` can additionally derive the layout from the ELF's +DWARF debug info and cross-check it against this table, so a firmware layout +change is caught instead of silently misread. + +Struct versions: the host must keep decoding EVERY version it has ever known, +not just the newest - an A/B bench session flashes old firmware whose struct +predates newer fields (e.g. v1 lacks the zero-cross jitter block), and the +harness must still read it. New fields are appended after ``host_cmd`` so its +offset never moves between versions and RESET_STATS works on any vintage. + +All fields are little-endian and naturally aligned (the struct is deliberately +not packed because Cortex-M0 cannot do unaligned word access). +""" +from __future__ import annotations + +import struct +from dataclasses import dataclass + +# ASCII "HWC1" little-endian == 0x31435748 +MAGIC = 0x31435748 +VERSION = 3 + +# (name, struct_code). Order and codes must match Inc/hwci_perf.h exactly. +# Pad fields are decoded then dropped from the public dict. +FIELDS_V1: list[tuple[str, str]] = [ + ("magic", "I"), + ("version", "H"), + ("size", "H"), + ("ctrl_exec_us_last", "H"), + ("ctrl_exec_us_max", "H"), + ("ctrl_period_us_last", "H"), + ("ctrl_period_us_max", "H"), + ("ctrl_period_us_min", "H"), + ("main_loop_us_last", "H"), + ("main_loop_us_max", "H"), + ("input", "H"), + ("duty_cycle", "H"), + ("e_rpm", "H"), + ("voltage_cv", "H"), + ("current_ca", "h"), + ("temperature_c", "h"), + ("bemf_timeout_state", "B"), + ("armed", "B"), + ("running", "B"), + ("_pad0", "B"), + ("_pad1", "H"), + ("loop_iters", "I"), + ("zero_cross_count", "I"), + ("commutation_interval", "I"), + ("commutation_interval_max", "I"), + ("update_count", "I"), + ("host_cmd", "I"), +] + +# v2 appends the zero-cross jitter block (see HWCI_PERF_ZC in Inc/hwci_perf.h). +FIELDS_V2: list[tuple[str, str]] = FIELDS_V1 + [ + ("zc_count", "I"), + ("zc_jitter_sum", "I"), + ("zc_interval_sum", "I"), + ("zc_jitter_max", "H"), + ("_pad2", "H"), +] + +# v3 appends bidirectional DShot (BDShot) RX/TX health (see Inc/hwci_perf.h). +FIELDS: list[tuple[str, str]] = FIELDS_V2 + [ + ("dshot_rx_good", "I"), + ("dshot_rx_bad", "I"), + ("dshot_tx_frames", "I"), + ("dshot_last_com_us", "H"), + ("dshot_telem_mode", "B"), + ("dshot_edt_mode", "B"), +] + +FIELDS_BY_VERSION: dict[int, list[tuple[str, str]]] = { + 1: FIELDS_V1, + 2: FIELDS_V2, + 3: FIELDS, +} + + +def _format(fields: list[tuple[str, str]]) -> str: + return "<" + "".join(code for _, code in fields) + + +_FORMAT_BY_VERSION = {v: _format(f) for v, f in FIELDS_BY_VERSION.items()} +SIZE_BY_VERSION = {v: struct.calcsize(fmt) for v, fmt in _FORMAT_BY_VERSION.items()} + +_FORMAT = _FORMAT_BY_VERSION[VERSION] +SIZE = SIZE_BY_VERSION[VERSION] # 96 bytes (v2: 80, v1: 64) +_NAMES = [name for name, _ in FIELDS] + +# magic + version + size header, enough to pick the right layout for the rest. +_HEADER = struct.Struct(" int: + return self.raw["ctrl_exec_us_max"] + + @property + def ctrl_period_us_max(self) -> int: + return self.raw["ctrl_period_us_max"] + + @property + def ctrl_period_us_min(self) -> int: + return self.raw["ctrl_period_us_min"] + + @property + def main_loop_us_max(self) -> int: + return self.raw["main_loop_us_max"] + + @property + def loop_iters(self) -> int: + return self.raw["loop_iters"] + + @property + def zero_cross_count(self) -> int: + return self.raw["zero_cross_count"] + + @property + def voltage(self) -> float: + """Battery voltage in volts (firmware reports centivolts).""" + return self.raw["voltage_cv"] / 100.0 + + @property + def current(self) -> float: + """Phase current in amps (firmware reports centiamps).""" + return self.raw["current_ca"] / 100.0 + + @property + def e_rpm(self) -> int: + """Electrical RPM (firmware reports eRPM/100).""" + return self.raw["e_rpm"] * 100 + + def mech_rpm(self, pole_pairs: int) -> float: + """Mechanical RPM derived from eRPM and motor pole pairs.""" + return self.e_rpm / pole_pairs if pole_pairs else 0.0 + + +def decode(data: bytes, *, host_monotonic: float | None = None, + validate: bool = True) -> PerfSample: + """Decode ``data`` into a :class:`PerfSample`. + + The struct version in the header selects the layout, so buffers from any + known firmware vintage decode correctly. ``data`` may be longer than the + selected layout (a v2-sized read of a v1 target); the excess is ignored. + """ + if len(data) < _HEADER.size: + raise PerfDecodeError(f"need at least {_HEADER.size} bytes, got {len(data)}") + magic, version, size = _HEADER.unpack_from(data) + if validate: + if magic != MAGIC: + raise PerfDecodeError( + f"bad magic 0x{magic:08x} (expected 0x{MAGIC:08x}); " + "is the firmware built with HWCI_PERF=1?") + if version not in FIELDS_BY_VERSION: + raise PerfDecodeError( + f"struct version {version} unknown to host " + f"(knows {sorted(FIELDS_BY_VERSION)}); update hwci/hwci/perf.py") + fields = FIELDS_BY_VERSION.get(version) or FIELDS + ver = version if version in FIELDS_BY_VERSION else VERSION + fmt, expected = _FORMAT_BY_VERSION[ver], SIZE_BY_VERSION[ver] + if len(data) < expected: + raise PerfDecodeError( + f"v{ver} struct needs {expected} bytes, got {len(data)}") + names = [name for name, _ in fields] + raw = dict(zip(names, struct.unpack(fmt, data[:expected]))) + if validate and raw["size"] != expected: + raise PerfDecodeError( + f"struct size {raw['size']} != host-expected {expected} for " + f"v{version}; firmware/host layout drift - rebuild or update " + "hwci/hwci/perf.py") + public = {k: v for k, v in raw.items() if not k.startswith("_pad")} + return PerfSample(raw=public, host_monotonic=host_monotonic) + + +def encode(raw: dict, version: int = VERSION) -> bytes: + """Inverse of :func:`decode` (used by the simulator and tests).""" + fields = FIELDS_BY_VERSION[version] + full = {name: 0 for name, _ in fields} + full.update({"magic": MAGIC, "version": version, + "size": SIZE_BY_VERSION[version]}) + full.update({k: v for k, v in raw.items() if k in full}) + return struct.pack(_FORMAT_BY_VERSION[version], + *(full[name] for name, _ in fields)) diff --git a/hwci/hwci/perf_reader.py b/hwci/hwci/perf_reader.py new file mode 100644 index 000000000..2c5d7368b --- /dev/null +++ b/hwci/hwci/perf_reader.py @@ -0,0 +1,92 @@ +"""Read and decode the firmware ``hwci_perf`` struct via a Debugger backend.""" +from __future__ import annotations + +import time +import warnings + +from . import elf, perf +from .debugger.base import Debugger, DebuggerError + +SYMBOL = "hwci_perf" +STRUCT_TAG = "hwci_perf_s" + + +class PerfReader: + """Locate ``hwci_perf`` once from the ELF, then sample it on demand. + + On construction the symbol address is read from ``elf_path``. If the ELF + carries DWARF (AM32 builds with ``-g``), the on-target layout is cross- + checked against :data:`hwci.perf.FIELDS` so a firmware/host layout drift is + caught loudly instead of silently misread. + """ + + def __init__(self, dbg: Debugger, elf_path: str, *, check_layout: bool = True): + self.dbg = dbg + self.elf_path = elf_path + sym = elf.find_symbol(elf_path, SYMBOL) + self.address = sym.address + # The ELF's struct size identifies the firmware's layout version: an + # A/B session flashes old firmware whose struct predates newer fields, + # and the harness must keep reading it (perf.decode is version-aware). + known = set(perf.SIZE_BY_VERSION.values()) + if sym.size and sym.size not in known: + raise perf.PerfDecodeError( + f"hwci_perf ELF size {sym.size} matches no known version " + f"({sorted(known)}); rebuild firmware or update hwci/hwci/perf.py") + self._read_size = sym.size or perf.SIZE + if check_layout: + self._check_layout() + + def _check_layout(self) -> None: + try: + members = {m.name: m for m in elf.struct_layout(self.elf_path, STRUCT_TAG)} + except elf.StructNotFoundError as e: + # DWARF exists but the struct DIE is gone (renamed tag, LTO/-g1 + # stripping types): decoding would proceed on faith exactly when + # the cross-check matters most. Fail hard. + raise perf.PerfDecodeError( + f"cannot cross-check hwci_perf layout: {e}") from e + except elf.ElfError as e: + warnings.warn(f"hwci_perf layout cross-check skipped ({e}); " + "relying on the canonical layout in hwci/hwci/perf.py") + return + # Pick the canonical layout matching the firmware's vintage by probing + # for a v2-only member, then verify every field of THAT layout. + fields = perf.FIELDS if "zc_count" in members else perf.FIELDS_V1 + off = 0 + import struct as _struct + for name, code in fields: + size = _struct.calcsize(code) + if not name.startswith("_pad"): + m = members.get(name) + if m is None or m.offset != off or m.size != size: + raise perf.PerfDecodeError( + f"firmware/host layout mismatch at {name!r}: " + f"ELF={m} canonical(off={off},size={size})") + off += size + + def read(self) -> perf.PerfSample: + data = self.dbg.read_memory(self.address, self._read_size) + return perf.decode(data, host_monotonic=time.monotonic()) + + def reset_stats(self, *, verify: bool = True, timeout_s: float = 1.0) -> None: + """Ask the firmware to clear its sticky min/max accumulators. + + With ``verify`` (the default) this polls until the firmware consumes + the command (it clears ``host_cmd`` within ~64 main-loop iterations). + A reset that silently never lands would let the previous run's maxima + pollute this run's gated metrics. + """ + cmd_addr = self.address + perf.HOST_CMD_OFFSET + self.dbg.write_u32(cmd_addr, perf.CMD_RESET_STATS) + if not verify: + return + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + val = int.from_bytes(self.dbg.read_memory(cmd_addr, 4), "little") + if val == perf.CMD_NONE: + return + time.sleep(0.01) + raise DebuggerError( + "firmware did not acknowledge RESET_STATS within " + f"{timeout_s}s (is the target running HWCI_PERF firmware?)") diff --git a/hwci/hwci/profiles/bdshot_smoke.yaml b/hwci/hwci/profiles/bdshot_smoke.yaml new file mode 100644 index 000000000..51b24ee18 --- /dev/null +++ b/hwci/hwci/profiles/bdshot_smoke.yaml @@ -0,0 +1,35 @@ +# SETUP B only — ARK FPV / BDShot owns the ESC signal pin. +# This profile is for optional SWD sampling while PX4 drives the motor +# (throttle_backend: none). It does NOT command the Flight Stand ESC out. +# For SETUP A free-run use noprop_smoke* with rig.flightstand / rig.yaml. +# See docs/BENCH_SETUPS.md. +name: bdshot_smoke +description: > + SETUP B (PX4 BDShot): gentle free-run shape for optional SWD logging while + ARK FPV drives BDShot. Harness throttle must be none — motor motion comes + from scripts/px4_motor_stream.py (or QGC), not this profile's throttle_cmd. + Do not use with Flight Stand throttle (SETUP A). ~35 s if used as a + timing template only. +sample_rate_hz: 100 +arm_settle_s: 3.0 +pole_pairs: 7 +demag_rpm_drop_fraction: 0.25 +demag_commutation_spike: 3.0 +steady_tail_fraction: 0.5 + +safety: + max_current_a: 50.0 + max_motor_temp_c: 90.0 + max_rpm: 55000 + max_thrust_n: 2.0 + +segments: + - {label: idle, throttle: 0.00, duration_s: 2.0} + - {label: ramp20, throttle: 0.20, duration_s: 3.0, ramp: true} + - {label: hold20, throttle: 0.20, duration_s: 4.0, steady: true} + - {label: ramp50, throttle: 0.50, duration_s: 4.0, ramp: true} + - {label: hold50, throttle: 0.50, duration_s: 5.0, steady: true} + - {label: ramp80, throttle: 0.80, duration_s: 4.0, ramp: true} + - {label: hold80, throttle: 0.80, duration_s: 5.0, steady: true} + - {label: rampdn, throttle: 0.00, duration_s: 4.0, ramp: true} + - {label: stop, throttle: 0.00, duration_s: 2.0} diff --git a/hwci/hwci/profiles/ci_smoke.yaml b/hwci/hwci/profiles/ci_smoke.yaml new file mode 100644 index 000000000..58807675a --- /dev/null +++ b/hwci/hwci/profiles/ci_smoke.yaml @@ -0,0 +1,22 @@ +name: ci_smoke +description: > + Fast, low-risk gate for CI. Arms, ramps to two modest steady points, returns + to idle. Validates the whole pipeline (build/flash/telemetry/perf) and catches + gross regressions in loop time, CPU load, and efficiency without stressing the + motor. ~25 s. +sample_rate_hz: 100 +arm_settle_s: 2.0 + +safety: + max_current_a: 35.0 + max_thrust_n: 15.0 + max_rpm: 30000 + +segments: + - {label: idle, throttle: 0.00, duration_s: 2.0} + - {label: ramp25, throttle: 0.25, duration_s: 3.0, ramp: true} + - {label: hold25, throttle: 0.25, duration_s: 5.0, steady: true} + - {label: ramp45, throttle: 0.45, duration_s: 3.0, ramp: true} + - {label: hold45, throttle: 0.45, duration_s: 5.0, steady: true} + - {label: rampdn, throttle: 0.00, duration_s: 3.0, ramp: true} + - {label: stop, throttle: 0.00, duration_s: 2.0} diff --git a/hwci/hwci/profiles/demag_step_stress.yaml b/hwci/hwci/profiles/demag_step_stress.yaml new file mode 100644 index 000000000..23e0fe878 --- /dev/null +++ b/hwci/hwci/profiles/demag_step_stress.yaml @@ -0,0 +1,27 @@ +name: demag_step_stress +description: > + Aggressive instantaneous throttle steps (no ramp) between low and high to + provoke loss of sync / demagnetization. Detection is host-side: RPM/thrust + collapse while throttle is commanded high, ESC eRPM vs stand RPM divergence, + commutation-interval spikes, and firmware bemf-timeout flags. Run with caution + and conservative current/thrust cutoffs. +sample_rate_hz: 200 +arm_settle_s: 2.0 +demag_commutation_spike: 3.0 +demag_rpm_drop_fraction: 0.25 + +safety: + max_current_a: 45.0 + max_thrust_n: 16.0 + max_rpm: 32000 + +segments: + - {label: idle, throttle: 0.00, duration_s: 2.0} + - {label: pre, throttle: 0.10, duration_s: 3.0, ramp: true} + - {label: step90a, throttle: 0.90, duration_s: 2.0} # snap 10->90 + - {label: drop10a, throttle: 0.10, duration_s: 2.0} # snap 90->10 + - {label: step95, throttle: 0.95, duration_s: 2.0} + - {label: drop10b, throttle: 0.10, duration_s: 2.0} + - {label: step100, throttle: 1.00, duration_s: 2.0} + - {label: drop05, throttle: 0.05, duration_s: 2.0} + - {label: rampdn, throttle: 0.00, duration_s: 2.0, ramp: true} diff --git a/hwci/hwci/profiles/efficiency_sweep.yaml b/hwci/hwci/profiles/efficiency_sweep.yaml new file mode 100644 index 000000000..87c334337 --- /dev/null +++ b/hwci/hwci/profiles/efficiency_sweep.yaml @@ -0,0 +1,40 @@ +name: efficiency_sweep +description: > + Steady-state staircase from 10% to 100% throttle. Each step dwells long enough + to settle, and the tail of each is used to compute thrust, electrical power and + efficiency (g/W) plus loop time / CPU load at that operating point. This is the + primary efficiency + performance baseline. + + Hold time is 6s. It was temporarily 10s while the ARK 4IN1 bench had severe + thrust-channel noise (sample CV up to 66%) that turned out to be prop-wake + impingement on the stand's mounting plate - the 5" disc sat entirely inside + the plate footprint blowing INTO it - not vibration to be averaged away. + With the prop reversed to exhaust into free air (thrust CV now 5.5-8.8%), + re-analysis of four 10s-hold captures truncated to 6s showed a 3s tail + changes gated points (>= 20W) by under +/-2% and leaves run-to-run + repeatability statistically unchanged (worst gated-point spread 9.8% vs + 9.1%), so the extra 4s/point (~40s of motor+battery per sweep) bought + nothing. See Thresholds.efficiency_drop_pct in hwci/baseline.py for the + matching tolerance history. +sample_rate_hz: 100 +arm_settle_s: 2.0 +steady_tail_fraction: 0.5 + +safety: + max_current_a: 40.0 + max_thrust_n: 16.0 + max_rpm: 32000 + +segments: + - {label: idle, throttle: 0.00, duration_s: 2.0} + - {label: t10, throttle: 0.10, duration_s: 6.0, ramp: true, steady: true} + - {label: t20, throttle: 0.20, duration_s: 6.0, steady: true} + - {label: t30, throttle: 0.30, duration_s: 6.0, steady: true} + - {label: t40, throttle: 0.40, duration_s: 6.0, steady: true} + - {label: t50, throttle: 0.50, duration_s: 6.0, steady: true} + - {label: t60, throttle: 0.60, duration_s: 6.0, steady: true} + - {label: t70, throttle: 0.70, duration_s: 6.0, steady: true} + - {label: t80, throttle: 0.80, duration_s: 6.0, steady: true} + - {label: t90, throttle: 0.90, duration_s: 6.0, steady: true} + - {label: t100, throttle: 1.00, duration_s: 6.0, steady: true} + - {label: rampdn, throttle: 0.00, duration_s: 4.0, ramp: true} diff --git a/hwci/hwci/profiles/noprop_baseline.yaml b/hwci/hwci/profiles/noprop_baseline.yaml new file mode 100644 index 000000000..bcb595aff --- /dev/null +++ b/hwci/hwci/profiles/noprop_baseline.yaml @@ -0,0 +1,27 @@ +name: noprop_baseline +description: > + No-prop performance baseline: steady staircase 10% to 50% throttle. Without a + prop there is no meaningful thrust/efficiency, but loop timing, CPU load, + commutation health, eRPM-vs-stand-RPM agreement, and no-load current per + operating point are all stable regression signals. Capped at 50% (a free + 1800KV bell on 6S already turns ~22k RPM there); raise only with a load. + ~40 s. +sample_rate_hz: 100 +arm_settle_s: 3.0 +steady_tail_fraction: 0.5 + +safety: + max_current_a: 10.0 # no-load should stay under ~1.5 A; trips on desync/stall + max_thrust_n: 3.0 # ~0 expected without a prop; trips on sensor faults + max_rpm: 28000 # expected ~22.5k mech at 50%; hard stop above + max_motor_temp_c: 80.0 # IR probe on the motor (also gates KISS telem if wired) + +segments: + - {label: idle, throttle: 0.00, duration_s: 2.0} + - {label: t10, throttle: 0.10, duration_s: 6.0, ramp: true, steady: true} + - {label: t20, throttle: 0.20, duration_s: 6.0, steady: true} + - {label: t30, throttle: 0.30, duration_s: 6.0, steady: true} + - {label: t40, throttle: 0.40, duration_s: 6.0, steady: true} + - {label: t50, throttle: 0.50, duration_s: 6.0, steady: true} + - {label: rampdn, throttle: 0.00, duration_s: 3.0, ramp: true} + - {label: stop, throttle: 0.00, duration_s: 2.0} diff --git a/hwci/hwci/profiles/noprop_smoke.yaml b/hwci/hwci/profiles/noprop_smoke.yaml new file mode 100644 index 000000000..bdc807609 --- /dev/null +++ b/hwci/hwci/profiles/noprop_smoke.yaml @@ -0,0 +1,23 @@ +name: noprop_smoke +description: > + Bring-up validation with NO propeller installed: arms, holds two gentle + throttle points, returns to idle. Verifies throttle path, arming, spin-up, + and all data channels with minimal mechanical risk (no-load current is a + few amps). Thrust/efficiency numbers are meaningless without a prop - this + profile validates the pipeline, not the powertrain. ~20 s. +sample_rate_hz: 100 +arm_settle_s: 3.0 + +safety: + max_current_a: 8.0 # no-load current should stay well under this + max_thrust_n: 2.0 # ~0 expected without a prop; trips on sensor faults + max_rpm: 25000 # 1800KV @ 6S free-runs ~45k at 100%; stay low + +segments: + - {label: idle, throttle: 0.00, duration_s: 2.0} + - {label: ramp10, throttle: 0.10, duration_s: 2.0, ramp: true} + - {label: hold10, throttle: 0.10, duration_s: 4.0, steady: true} + - {label: ramp20, throttle: 0.20, duration_s: 2.0, ramp: true} + - {label: hold20, throttle: 0.20, duration_s: 4.0, steady: true} + - {label: rampdn, throttle: 0.00, duration_s: 2.0, ramp: true} + - {label: stop, throttle: 0.00, duration_s: 2.0} diff --git a/hwci/hwci/report.py b/hwci/hwci/report.py new file mode 100644 index 000000000..112402fd0 --- /dev/null +++ b/hwci/hwci/report.py @@ -0,0 +1,114 @@ +"""Render run metrics + regression comparison to Markdown (and optional plots).""" +from __future__ import annotations + +from pathlib import Path + + +def _fmt(v) -> str: + if v is None: + return "-" + if isinstance(v, float): + return f"{v:.3f}" if abs(v) < 1000 else f"{v:.0f}" + return str(v) + + +def render_markdown(metrics: dict, comparison: dict | None = None, + meta: dict | None = None) -> str: + out: list[str] = [] + out.append("# AM32 Hardware-CI Report\n") + if meta: + out.append("## Run\n") + for k in ("target", "profile", "mode", "git_sha", "firmware_version", + "motor", "prop", "pole_pairs", "timestamp", "aborted", + "perf_read_errors"): + if k in meta and meta[k] is not None: + out.append(f"- **{k}**: {meta[k]}") + out.append("") + + s = metrics["summary"] + if comparison is not None: + verdict = "✅ PASS" if comparison["passed"] else "❌ FAIL" + out.append(f"## Verdict: {verdict}\n") + + out.append("## Summary\n") + out.append("| metric | value |") + out.append("|---|---|") + for k, v in s.items(): + out.append(f"| {k} | {_fmt(v)} |") + out.append("") + + pts = metrics.get("steady_points", []) + if pts: + out.append("## Steady-state operating points\n") + cols = ["segment", "throttle", "rpm", "thrust_gf", "current_a", + "voltage_v", "elec_power_w", "eff_gf_per_w", + "ctrl_exec_us_max", "cpu_load_pct", + "zc_jitter_pct", "zc_jitter_max_pct"] + out.append("| " + " | ".join(cols) + " |") + out.append("|" + "---|" * len(cols)) + for p in pts: + out.append("| " + " | ".join(_fmt(p.get(c)) for c in cols) + " |") + out.append("") + + d = metrics.get("demag", {}) + out.append("## Demag / desync\n") + out.append(f"- events: **{d.get('event_count', 0)}**") + out.append(f"- bemf-timeout samples: {d.get('bemf_timeout_samples', 0)}") + out.append(f"- commutation-spike samples: {d.get('comm_spike_samples', 0)}") + out.append(f"- ESC-eRPM vs stand-RPM mismatch samples: " + f"{d.get('esc_rpm_mismatch_samples', 0)}") + out.append("") + + if comparison is not None: + out.append("## Regression checks (vs baseline)\n") + out.append("| check | baseline | current | pass | rule |") + out.append("|---|---|---|---|---|") + for c in comparison["checks"]: + mark = "✅" if c["pass"] else "❌" + out.append(f"| {c['name']} | {_fmt(c['baseline'])} | " + f"{_fmt(c['current'])} | {mark} | {c['note']} |") + out.append("") + + return "\n".join(out) + + +def write_report(run_dir: str | Path, metrics: dict, + comparison: dict | None = None, meta: dict | None = None, + plots: bool = True) -> Path: + run_dir = Path(run_dir) + run_dir.mkdir(parents=True, exist_ok=True) + md = render_markdown(metrics, comparison, meta) + report_path = run_dir / "report.md" + report_path.write_text(md) + if plots: + try: + _write_plots(run_dir, metrics) + except Exception: + pass # plotting is best-effort / optional + return report_path + + +def _write_plots(run_dir: Path, metrics: dict) -> None: + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + pts = metrics.get("steady_points", []) + if not pts: + return + thr = [p["throttle"] * 100 for p in pts] + + fig, axes = plt.subplots(2, 2, figsize=(10, 7)) + axes[0, 0].plot(thr, [p["thrust_gf"] for p in pts], "o-") + axes[0, 0].set(title="Thrust", xlabel="throttle %", ylabel="gf") + axes[0, 1].plot(thr, [p["eff_gf_per_w"] for p in pts], "o-", color="green") + axes[0, 1].set(title="Efficiency", xlabel="throttle %", ylabel="g/W") + axes[1, 0].plot(thr, [p["cpu_load_pct"] for p in pts], "o-", color="red") + axes[1, 0].set(title="CPU load", xlabel="throttle %", ylabel="%") + axes[1, 1].plot(thr, [p["ctrl_exec_us_max"] for p in pts], "o-", color="purple") + axes[1, 1].axhline(50, ls="--", color="gray", label="20kHz budget") + axes[1, 1].set(title="Control-loop exec", xlabel="throttle %", ylabel="us") + axes[1, 1].legend() + fig.tight_layout() + fig.savefig(run_dir / "summary.png", dpi=110) + plt.close(fig) diff --git a/hwci/hwci/runner.py b/hwci/hwci/runner.py new file mode 100644 index 000000000..1e4926b71 --- /dev/null +++ b/hwci/hwci/runner.py @@ -0,0 +1,527 @@ +"""Test runner: execute a profile, sampling stand + ESC telemetry + perf struct. + +The runner is source-agnostic: it drives a :class:`ThrottleSource`, reads a +:class:`ThrustStand`, and calls two callables for the perf struct and ESC +telemetry. :func:`build_sim_sources` wires all of these to one shared +:class:`~hwci.sim.RigSimulator` for offline runs; :func:`build_live_sources` +wires them to OpenOCD + serial + the gRPC stand on the rig. + +Safety: the profile's :class:`~hwci.flightstand.base.SafetyLimits` are enforced +HERE, on every sample, against both the stand reading and the ESC telemetry. +Backends may additionally enforce them, but the runner check is the one that +exists on every rig (the vendor gRPC API has no mapped set-limit RPC yet). + +Pre-flight: :func:`check_battery` gates a hardware run on pack voltage BEFORE +the throttle is ever armed (see :func:`build_live_sources`). It is opt-in +(``--battery-cells``) and separate from the per-sample ``SafetyLimits`` above - +a low-but-not-crashing pack should never even start a test, both to protect +the pack and because a sagging supply quietly corrupts efficiency data. +:func:`tare_for_run` then zeroes the load cells on every hardware run that has +a stand (default-on, ``--no-tare`` to skip) - with the ESC signal already up +at zero throttle, because AM32 beeps the motor whenever it has no input. +""" +from __future__ import annotations + +import sys +import threading +import time +from dataclasses import dataclass, field +from typing import Callable, Optional + +from . import perf +from .config import Profile, RigConfig +from .esc_telem.kiss import KissFrame, KissStream, parse_frame +from .flightstand.base import SafetyLimits, StandSafetyTripped, StandSample, ThrustStand +from .metrics import tail_start_index +from .model import RunResult, make_row +from .perf import PerfSample +from .perf_reader import PerfReader +from .throttle.base import ThrottleSource + +PerfSourceFn = Callable[[], Optional[PerfSample]] +TelemSourceFn = Callable[[], Optional[KissFrame]] + + +@dataclass +class Sources: + throttle: ThrottleSource + stand: Optional[ThrustStand] + perf_source: PerfSourceFn + telem_source: TelemSourceFn + perf_reader: Optional[PerfReader] = None + closers: list = field(default_factory=list) + + def close(self) -> None: + for c in reversed(self.closers): + try: + c() + except Exception: + pass + + +# Ticks of margin between issuing a mid-segment perf-stats reset and the +# metrics tail window's first sample - see the call site in run_profile for +# why this must be a margin, not an exact boundary match. +RESET_MARGIN_TICKS = 20 + + +def _tail_reset_tick(n: int, steady_tail_fraction: float) -> int: + """Tick within a steady segment to reset perf stats at, strictly before + the metrics tail window - computed from the SAME tail_start_index() the + metrics module uses, so the two can never disagree by a rounding tick.""" + return max(0, tail_start_index(n, steady_tail_fraction) - RESET_MARGIN_TICKS) + + +def _segment_throttle(seg, tick_in_seg: int, n_ticks: int, prev: float) -> float: + if not seg.ramp or n_ticks <= 1: + return seg.throttle + frac = tick_in_seg / (n_ticks - 1) + return prev + (seg.throttle - prev) * min(1.0, frac) + + +def enforce_safety(limits: SafetyLimits, stand: StandSample | None, + telem: KissFrame | None) -> None: + """Host-side safety check against every channel that produced data.""" + if stand is not None: + limits.check(thrust_n=stand.thrust_n, current_a=stand.current_a, + rpm=stand.rpm, voltage_v=stand.voltage_v, + temp_c=stand.motor_temp_c) + if telem is not None: + limits.check(current_a=telem.current_a, temp_c=telem.temperature_c) + + +# Default per-cell low-voltage threshold for check_battery(). Matches AM32 +# firmware's own default (Src/main.c: `low_cell_volt_cutoff = 330` -> 3.30 +# V/cell), so the harness refuses to start a test at roughly the same pack +# voltage the ESC would eventually cut power at mid-run anyway. +DEFAULT_MIN_CELL_VOLTAGE = 3.3 + + +class BatteryTooLowError(RuntimeError): + """Raised before a run is armed: the pack is already at/below the + minimum for its declared cell count, or its voltage can't be verified at + all. Fails closed - a test must not start on a battery this low, both to + avoid over-discharging it and because a sagging supply quietly corrupts + efficiency data.""" + + +def _live_voltage(stand: ThrustStand | None, + perf_source: PerfSourceFn) -> float | None: + """Best pack-voltage reading available before the throttle is armed: the + stand's HV bus channel if one is wired, else the ESC's own ADC via the + perf struct (already alive by the time build_live_sources calls this - + it runs after _ensure_app_alive). None if neither is available/readable.""" + if stand is not None: + return stand.read_sample().voltage_v + pf = _safe(perf_source) + return pf.voltage if pf is not None else None + + +def check_battery(voltage_v: float | None, battery_cells: int, + min_cell_voltage: float = DEFAULT_MIN_CELL_VOLTAGE) -> None: + """Refuse to proceed if the pack is at/below a safe minimum for its + declared cell count. Only called when the caller passed + ``--battery-cells`` - this check is opt-in, not a default gate.""" + minimum = battery_cells * min_cell_voltage + if voltage_v is None: + raise BatteryTooLowError( + "cannot verify battery voltage before starting (no stand or " + "perf voltage channel available on this rig) - wire a voltage " + "channel or drop --battery-cells to skip this check") + if voltage_v < minimum: + raise BatteryTooLowError( + f"battery {voltage_v:.2f} V < minimum {minimum:.2f} V for a " + f"{battery_cells}S pack at {min_cell_voltage:.2f} V/cell - " + "charge or swap the pack before running a test") + + +# Pre-run tare choreography. AM32 beeps the motor whenever it sees NO input +# signal (the disconnect beacon) and again as it arms - each beep is a real +# torque pulse through the mount, so a tare taken on a signal-less ESC bakes +# that twitching into the load-cell zero. Bring the signal up at zero +# throttle FIRST (the beacon stops), wait out the arm tune, and only then +# zero the load cells on a mechanically quiet rig. +ARM_TUNE_SETTLE_S = 2.0 # arm tune keeps shaking the motor after arm() returns +TARE_SETTLE_S = 1.5 # post-tare readings stay noisy for ~1-2 s (bench) + + +def tare_for_run(stand: ThrustStand, throttle: ThrottleSource, *, + settle: Callable[[float], None] = time.sleep) -> float | None: + """Zero the load cells with the ESC held quiet at zero throttle (see the + choreography note above). Returns the post-tare thrust residual in gf so + the caller can surface it, or None if it can't be read - the residual + read is best-effort, the tare itself is not.""" + throttle.arm() # signal present at zero: beacon stops, ESC arms + settle(ARM_TUNE_SETTLE_S) + stand.tare() + settle(TARE_SETTLE_S) + try: + return stand.read_sample().thrust_n * 1000.0 / 9.80665 + except Exception: + return None + + +class _CachedPoller: + """Background thread that keeps the most recent value of a slow source. + + An SWD struct read through ST-Link costs milliseconds; done inline it + would consume the whole tick budget at 100-200 Hz and add jitter to every + sample. Read errors are counted, never swallowed silently. + """ + + def __init__(self, fn, *, interval_s: float = 0.002, + max_age_s: float = 1.0, name: str = "poller"): + self._fn = fn + self._interval = interval_s + self._max_age = max_age_s + self._latest = None + self._latest_at = float("-inf") + self._errors = 0 + self._last_error: Exception | None = None + self._stop = threading.Event() + self._thread = threading.Thread(target=self._loop, daemon=True, name=name) + self._thread.start() + + def _loop(self) -> None: + while not self._stop.is_set(): + try: + value = self._fn() + except Exception as e: + self._errors += 1 + self._last_error = e + else: + self._latest = value + self._latest_at = time.monotonic() + self._stop.wait(self._interval) + + def latest(self): + """Most recent value, or None once the source has been dead for + max_age_s (a stale sample repeated forever would defeat the + channel-coverage gate downstream).""" + if time.monotonic() - self._latest_at > self._max_age: + return None + return self._latest + + @property + def errors(self) -> int: + return self._errors + + @property + def last_error(self) -> Exception | None: + return self._last_error + + def close(self) -> None: + self._stop.set() + self._thread.join(timeout=1.0) + + +def run_profile(profile: Profile, sources: Sources, *, + realtime: bool = True, meta: dict | None = None) -> RunResult: + period = 1.0 / profile.sample_rate_hz + rows: list[dict] = [] + aborted: str | None = None + + sources.throttle.arm() + if sources.perf_reader is not None: + sources.perf_reader.reset_stats() # measure worst-case for THIS run + + # Slow live sources move to a caching poller thread so the tick loop stays + # deterministic. The sim path stays inline (cheap and reproducible). + perf_poller: _CachedPoller | None = None + perf_get: Callable[[], PerfSample | None] + telem_errors = 0 + if realtime and sources.perf_reader is not None: + perf_poller = _CachedPoller(sources.perf_source, name="perf-poller") + perf_get = perf_poller.latest + else: + def perf_get(): + return _safe(sources.perf_source) + + start = time.monotonic() + tick = 0 + prev_throttle = 0.0 + try: + for seg in profile.segments: + n = max(1, round(seg.duration_s * profile.sample_rate_hz)) + # Reset the firmware's sticky min/max accumulators BEFORE a + # steady segment's measurement tail begins (with a margin - see + # _tail_reset_tick), so the tail's worst case reflects THIS + # operating point - not the spin-up transient of the run so far + # (~700-950 us on the bench, varying 30%+ run-to-run, which would + # drown steady loop-time regressions). + # + # The reset must land strictly BEFORE the tail's first sample, + # not AT it: reset_stats() clears the firmware register over an + # SWD round trip, and in realtime mode perf_get() reads an + # independent background poller's CACHED value (_CachedPoller, + # ~2ms interval) - so the tick that ISSUES the reset can still + # observe the pre-reset cached sample. Observed on the bench: an + # 877us arming-tune-transient value latched at t10's first tick + # persisted for the segment's first 5s and was still visible in + # the very sample the reset was issued on, making the "steady" + # max equal the raw run max. A margin many times the poller + # interval (and typical SWD round trip) makes this exclusion + # instead of a race. + tail_reset_tick = (_tail_reset_tick(n, profile.steady_tail_fraction) + if seg.steady and sources.perf_reader is not None + else None) + for i in range(n): + t_sched = tick * period + if realtime: + target = start + t_sched + delay = target - time.monotonic() + if delay > 0: + time.sleep(delay) + if tail_reset_tick is not None and i == tail_reset_tick: + try: + sources.perf_reader.reset_stats(verify=False) + except Exception: + pass # a missed reset degrades one segment, not the run + throttle = _segment_throttle(seg, i, n, prev_throttle) + sources.throttle.set(throttle) + stand = sources.stand.read_sample() if sources.stand is not None else None + pf = perf_get() + tm = _safe(sources.telem_source) + enforce_safety(profile.safety, stand, tm) + # Record the ACTUAL sample time: after a host stall the + # schedule time would lie about how much wall clock the + # sample covers (and corrupt counter-rate math downstream). + t = (time.monotonic() - start) if realtime else t_sched + rows.append(make_row(t, seg.label, throttle, stand, tm, pf)) + tick += 1 + prev_throttle = seg.throttle + except StandSafetyTripped as e: + aborted = f"safety: {e}" + finally: + try: + sources.throttle.disarm() + finally: + if perf_poller is not None: + perf_poller.close() + + full_meta = { + "profile": profile.name, + "sample_rate_hz": profile.sample_rate_hz, + "n_samples": len(rows), + "aborted": aborted, + "wall_time_s": round(time.monotonic() - start, 3), + "perf_read_errors": perf_poller.errors if perf_poller is not None else 0, + } + if perf_poller is not None and perf_poller.last_error is not None: + full_meta["perf_last_error"] = repr(perf_poller.last_error) + if meta: + full_meta.update(meta) + return RunResult(meta=full_meta, rows=rows) + + +def _safe(fn): + try: + return fn() + except Exception: + return None + + +# -------------------------------------------------------------------------- +# Source builders +# -------------------------------------------------------------------------- +def build_sim_sources(rig: RigConfig, profile: Profile, *, + demag_prone: bool = True) -> Sources: + """All three channels fed by one RigSimulator (deterministic, no hardware).""" + from .flightstand.simulator import SimulatedStand + from .sim import MotorParams, RigSimulator + from .throttle.flightstand_src import FlightStandThrottle + + period = 1.0 / profile.sample_rate_hz + sim = RigSimulator(params=MotorParams(pole_pairs=rig.pole_pairs, + demag_prone=demag_prone)) + stand = SimulatedStand(sim, fixed_dt=period).open() + stand.set_safety_limits(profile.safety) + throttle = FlightStandThrottle(stand, arm_settle_s=0.0) + return Sources( + throttle=throttle, + stand=stand, + perf_source=lambda: perf.decode(sim.perf_bytes()), + telem_source=lambda: parse_frame(sim.kiss_bytes()), + closers=[stand.close], + ) + + +def _ensure_app_alive(dbg, perf_reader: PerfReader, + throttle: ThrottleSource) -> None: + """Get the ESC out of the AM32 bootloader and into the app. + + The bootloader only jumps to the app when the throttle signal line idles + LOW at boot. A floating line reads high, and an ACTIVE DShot output keeps + the line high 40-75% of each frame - either way the ESC parks in the + bootloader after a flash/power-cycle and the run would produce no perf + data and never arm. Detected via the hwci_perf magic: quiesce the + throttle source (signal dropped, line driven low), reset, and wait for + the app to publish the magic. The throttle is re-activated later by + ``arm()``. + """ + from .perf import PerfDecodeError + + def app_alive() -> bool: + # Only a decode failure means "bootloader/no instrumentation"; + # a debugger error is a different fault and must propagate. + try: + perf_reader.read() + return True + except PerfDecodeError: + return False + + if app_alive(): + return + for _attempt in range(2): + throttle.quiesce() # drop the signal so the line is driven low + time.sleep(0.2) # let the output state settle + dbg.reset_run() + deadline = time.monotonic() + 8.0 # boot + arming tune + while time.monotonic() < deadline: + time.sleep(0.25) + if app_alive(): + return + raise RuntimeError( + "ESC app never came up (hwci_perf magic invalid after reset). " + "Either the flashed firmware was built without HWCI_PERF=1, or the " + "ESC is stuck in the AM32 bootloader because the throttle signal " + "line idles high at reset (check the stand's ESC output wiring and " + "that the throttle backend can drive it).") + + +class _SerialTelemetry: + """Background thread holding the most recent KISS frame from a serial port.""" + + def __init__(self, port: str, baud: int): + import serial + self._ser = serial.Serial(port, baud, timeout=0.05) + self._stream = KissStream() + self._latest: KissFrame | None = None + self._stop = threading.Event() + self._thread = threading.Thread(target=self._loop, daemon=True) + self._thread.start() + + def _loop(self) -> None: + while not self._stop.is_set(): + chunk = self._ser.read(64) + if chunk: + for frame in self._stream.feed(chunk): + self._latest = frame + + def latest(self) -> KissFrame | None: + return self._latest + + def close(self) -> None: + self._stop.set() + self._thread.join(timeout=1.0) + self._ser.close() + + +def build_live_sources(rig: RigConfig, profile: Profile, *, + battery_cells: int | None = None, + min_cell_voltage: float = DEFAULT_MIN_CELL_VOLTAGE, + tare: bool = True) -> Sources: + """Wire OpenOCD + serial telemetry + gRPC/external throttle on the rig. + + Backend values dispatch STRICTLY: an unknown value raises instead of + falling back to a simulator (fabricated data gating a hardware run is the + one unrecoverable failure mode). ``none`` disables a channel explicitly. + + If ``battery_cells`` is given, pack voltage is checked against + ``battery_cells * min_cell_voltage`` before the throttle is armed (see + :func:`check_battery`) - the run refuses to start on a pack that's + already too low. + + Unless ``tare`` is False, a stand's load cells are zeroed as the last + pre-flight step via :func:`tare_for_run` (ESC signal up at zero throttle + first, so AM32's beacon/arm beeps don't shake the cells mid-tare). A tare + that fails aborts the run: after any mechanical change an untared cell + reads a bogus thrust offset, which is worse than no run. + """ + closers: list = [] + + # --- thrust stand --- + stand: ThrustStand | None + if rig.stand_backend == "grpc": + from .flightstand.grpc_client import FlightStandGrpc, SignalMap + sig = SignalMap(**rig.stand_signals) if rig.stand_signals else SignalMap() + stand = FlightStandGrpc(rig.stand_host, rig.stand_port, signals=sig).open() + stand.set_safety_limits(profile.safety) + closers.append(stand.close) + elif rig.stand_backend == "none": + stand = None + else: + raise ValueError( + f"stand_backend {rig.stand_backend!r} is not a live backend " + "(expected 'grpc' or 'none')") + + # --- throttle source --- + if rig.throttle_backend == "external": + from .throttle.external import ExternalSerialThrottle + throttle = ExternalSerialThrottle(rig.throttle_port, rig.throttle_baud) + elif rig.throttle_backend == "flightstand": + if stand is None: + raise ValueError( + "throttle_backend 'flightstand' needs stand_backend 'grpc'") + from .throttle.flightstand_src import FlightStandThrottle + throttle = FlightStandThrottle(stand, arm_settle_s=profile.arm_settle_s) + elif rig.throttle_backend == "none": + # SETUP B: ARK FPV (or other host) owns the ESC signal pin. Harness + # may still flash + read SWD; do not call set() expecting motion. + from .throttle.null import NullThrottle + throttle = NullThrottle() + else: + raise ValueError( + f"throttle_backend {rig.throttle_backend!r} is not a live backend " + "(expected 'flightstand', 'external', or 'none')") + closers.append(throttle.close) + + # --- perf struct via debugger --- + perf_reader = None + perf_source: PerfSourceFn = lambda: None + if rig.debugger_backend == "openocd": + from .debugger.openocd import OpenOcdDebugger + elf = rig.resolved_elf() + if elf is None: + raise FileNotFoundError( + f"no ELF for target {rig.target} in {rig.resolved_obj_dir()}; " + "build with HWCI_PERF=1 first") + dbg = OpenOcdDebugger(rig.openocd_configs, openocd_bin=rig.openocd_bin, + search_dirs=rig.openocd_search_dirs).open() + perf_reader = PerfReader(dbg, str(elf)) + perf_source = perf_reader.read + closers.append(dbg.close) + elif rig.debugger_backend != "none": + raise ValueError( + f"debugger_backend {rig.debugger_backend!r} is not a live backend " + "(expected 'openocd' or 'none')") + + # --- ESC telemetry --- + telem_source: TelemSourceFn = lambda: None + if rig.telem_backend == "serial": + telem = _SerialTelemetry(rig.telem_port, rig.telem_baud) + telem_source = telem.latest + closers.append(telem.close) + elif rig.telem_backend != "none": + raise ValueError( + f"telem_backend {rig.telem_backend!r} is not a live backend " + "(expected 'serial' or 'none')") + + sources = Sources(throttle=throttle, stand=stand, perf_source=perf_source, + telem_source=telem_source, perf_reader=perf_reader, + closers=closers) + try: + if perf_reader is not None: + _ensure_app_alive(dbg, perf_reader, throttle) + if battery_cells is not None: + check_battery(_live_voltage(stand, perf_source), battery_cells, + min_cell_voltage) + if tare and stand is not None: + residual_gf = tare_for_run(stand, throttle) + if residual_gf is not None: + print(f"tared load cells (ESC armed quiet at zero): " + f"residual {residual_gf:+.1f} gf", file=sys.stderr) + except Exception: + sources.close() + raise + return sources diff --git a/hwci/hwci/sim.py b/hwci/hwci/sim.py new file mode 100644 index 000000000..febbb8311 --- /dev/null +++ b/hwci/hwci/sim.py @@ -0,0 +1,246 @@ +"""Offline rig simulator. + +Produces self-consistent data across all three measurement channels from a +single throttle command: + + * thrust-stand sample (thrust, torque, RPM, V, A) + * ESC KISS telemetry frame (temp, V, A, consumption, eRPM) + * firmware ``hwci_perf`` struct bytes (loop times, counters, snapshot) + +It is a phenomenological model, not a high-fidelity one - enough to exercise the +whole harness (metrics, baseline, report, runner) without hardware and to make +demag-detection logic testable by injecting desync events on aggressive steps. +""" +from __future__ import annotations + +import math +import random +from dataclasses import dataclass, field + +from . import perf +from .esc_telem.kiss import encode_frame +from .flightstand.base import StandSample + + +@dataclass +class MotorParams: + pole_pairs: int = 7 + kv: float = 1900.0 # rpm / V + batt_voltage: float = 16.8 # 4S nominal + internal_resistance: float = 0.02 # ohm (battery + wiring) + throttle_rpm_fraction: float = 0.85 + ct: float = 1.6e-8 # thrust [N] = ct * rpm^2 + cq: float = 1.43e-10 # torque [Nm] = cq * rpm^2 + motor_efficiency: float = 0.82 + idle_power_w: float = 3.0 + tau_s: float = 0.08 # spool-up time constant + ambient_temp_c: float = 25.0 + # demag behaviour + demag_prone: bool = False + demag_step_threshold: float = 0.35 # throttle jump that can desync + demag_current_a: float = 18.0 # current above which a jump desyncs + desync_ticks: int = 8 + + +@dataclass +class RigSimulator: + params: MotorParams = field(default_factory=MotorParams) + seed: int = 1234 + noise: float = 0.01 # fractional measurement noise + + def __post_init__(self): + self._rng = random.Random(self.seed) + self.rpm = 0.0 + self.throttle = 0.0 + self._prev_cmd = 0.0 + self.consumption_mah = 0.0 + self.temp_c = self.params.ambient_temp_c + self.loop_iters = 0 + self.zero_cross_count = 0 + self.update_count = 0 + self.desync_remaining = 0 + self.desync_count = 0 + self._ctrl_exec_max = 0 + self._commutation_max = 0 + # zero-cross jitter accumulators (perf struct v2) + self.zc_count = 0 + self.zc_jitter_sum = 0 + self.zc_interval_sum = 0 + self.zc_jitter_max = 0 + # BDShot counters (perf struct v3) — phenomenological, for harness tests + self.dshot_rx_good = 0 + self.dshot_rx_bad = 0 + self.dshot_tx_frames = 0 + + # --- model ------------------------------------------------------- + def _rpm_max(self) -> float: + p = self.params + return p.kv * p.batt_voltage * p.throttle_rpm_fraction + + def step(self, dt: float, throttle: float) -> None: + p = self.params + throttle = max(0.0, min(1.0, throttle)) + cmd_jump = throttle - self._prev_cmd + + # Detect a demag/desync trigger: a big, fast throttle increase into a + # high-current regime on a demag-prone motor. + target_rpm = throttle * self._rpm_max() + provisional_current = self._current_for_rpm(target_rpm, p.batt_voltage) + if (p.demag_prone and self.desync_remaining == 0 + and cmd_jump > p.demag_step_threshold + and provisional_current > p.demag_current_a): + self.desync_remaining = p.desync_ticks + self.desync_count += 1 + + if self.desync_remaining > 0: + # Loss of sync: rotor falls back, electrical drive flails. + self.rpm *= 0.7 + self.desync_remaining -= 1 + else: + alpha = 1.0 - math.exp(-dt / max(p.tau_s, 1e-3)) + self.rpm += (target_rpm - self.rpm) * alpha + + self.rpm = max(self.rpm, 0.0) + self.throttle = throttle + self._prev_cmd = throttle + + # bookkeeping that the perf struct exposes + erev_per_s = self.rpm * p.pole_pairs / 60.0 + n_comm = int(erev_per_s * 6.0 * dt) + self.zero_cross_count += n_comm + # zero-cross jitter accumulators: firmware gates on zero_crosses >= + # 100 (startup/desync-recovery excluded), deviation ~0.5% of the + # commutation interval in clean running, ballooning during a desync + if n_comm > 0 and self.zero_cross_count >= 100: + interval = int((1.0 / (erev_per_s * 6.0)) / 0.5e-6) + frac = 0.05 if self.desync_remaining > 0 else 0.005 + dev = max(1, int(interval * frac * self._rng.uniform(0.3, 1.7))) + self.zc_count = (self.zc_count + n_comm) & 0xFFFFFFFF + self.zc_jitter_sum = (self.zc_jitter_sum + dev * n_comm) & 0xFFFFFFFF + self.zc_interval_sum = (self.zc_interval_sum + interval * n_comm) & 0xFFFFFFFF + self.zc_jitter_max = min(max(self.zc_jitter_max, dev), 0xFFFF) + # idle loop iterations: ~120 kHz when idle, dropping with motor load + idle_hz = 120000.0 * (1.0 - 0.25 * throttle) + self.loop_iters += int(idle_hz * dt) + self.update_count += int(idle_hz * dt) + # BDShot: ~500 Hz command stream; RX good + TX reply when armed/running + n_ds = max(1, int(500.0 * dt)) + self.dshot_rx_good = (self.dshot_rx_good + n_ds) & 0xFFFFFFFF + if self.rpm > 100: + self.dshot_tx_frames = (self.dshot_tx_frames + n_ds) & 0xFFFFFFFF + # current draw heats the ESC slowly + cur = self.current + self.temp_c += (p.ambient_temp_c + 0.9 * cur - self.temp_c) * min(1.0, dt / 5.0) + self.consumption_mah += cur * dt / 3.6 # A*s -> mAh + + def _current_for_rpm(self, rpm: float, v: float) -> float: + p = self.params + torque = p.cq * rpm * rpm + omega = rpm * 2.0 * math.pi / 60.0 + mech = torque * omega + elec = mech / p.motor_efficiency + p.idle_power_w + return elec / max(v, 1.0) + + # --- derived measurements --------------------------------------- + @property + def thrust_n(self) -> float: + return self.params.ct * self.rpm * self.rpm + + @property + def torque_nm(self) -> float: + return self.params.cq * self.rpm * self.rpm + + @property + def current(self) -> float: + p = self.params + cur = self._current_for_rpm(self.rpm, p.batt_voltage) + if self.desync_remaining > 0: + cur *= 1.6 # current spike during desync + return cur + + @property + def voltage(self) -> float: + p = self.params + return p.batt_voltage - self.current * p.internal_resistance + + @property + def e_rpm(self) -> float: + return self.rpm * self.params.pole_pairs + + def _n(self, value: float) -> float: + """Apply multiplicative measurement noise.""" + if self.noise <= 0: + return value + return value * (1.0 + self._rng.uniform(-self.noise, self.noise)) + + # --- channel outputs -------------------------------------------- + def stand_sample(self, t: float) -> StandSample: + return StandSample( + t=t, + throttle=self.throttle, + thrust_n=self._n(self.thrust_n), + torque_nm=self._n(self.torque_nm), + rpm=self._n(self.rpm), + voltage_v=self._n(self.voltage), + current_a=self._n(self.current), + ) + + def kiss_bytes(self) -> bytes: + return encode_frame( + temperature_c=int(self.temp_c), + voltage_cv=int(self.voltage * 100), + current_ca=int(self.current * 100), + consumption_mah=int(self.consumption_mah), + erpm100=int(self.e_rpm / 100), + ) + + def perf_bytes(self) -> bytes: + p = self.params + erev_per_s = self.e_rpm / 60.0 + commutations_per_s = erev_per_s * 6.0 + if commutations_per_s > 1.0: + comm_interval = int((1.0 / commutations_per_s) / 0.5e-6) + else: + comm_interval = 0xFFFF + if self.desync_remaining > 0: + comm_interval = min(0xFFFFFF, comm_interval * 8) + self._commutation_max = max(self._commutation_max, comm_interval) + ctrl_exec = 16 + int(self._rng.uniform(0, 6)) + self._ctrl_exec_max = max(self._ctrl_exec_max, ctrl_exec) + return perf.encode({ + "ctrl_exec_us_last": ctrl_exec, + "ctrl_exec_us_max": self._ctrl_exec_max, + "ctrl_period_us_last": 50, + "ctrl_period_us_max": 52, + "ctrl_period_us_min": 48, + "main_loop_us_last": 5, + "main_loop_us_max": 9, + "input": int(self.throttle * 2000), + "duty_cycle": int(self.throttle * 2000), + "e_rpm": int(self.e_rpm / 100) & 0xFFFF, + "voltage_cv": int(self.voltage * 100) & 0xFFFF, + "current_ca": int(self.current * 100) & 0x7FFF, + "temperature_c": int(self.temp_c), + "bemf_timeout_state": 1 if self.desync_remaining > 0 else 0, + "armed": 1, + "running": 1 if self.rpm > 100 else 0, + "loop_iters": self.loop_iters & 0xFFFFFFFF, + # firmware clamps zero_crosses at 10000 (and resets it on + # desync/stop) - mirror the saturation so host logic tested + # against the sim cannot assume a monotonic counter + "zero_cross_count": min(self.zero_cross_count, 10000), + "commutation_interval": comm_interval, + "commutation_interval_max": self._commutation_max, + "update_count": self.update_count & 0xFFFFFFFF, + "host_cmd": 0, + "zc_count": self.zc_count, + "zc_jitter_sum": self.zc_jitter_sum, + "zc_interval_sum": self.zc_interval_sum, + "zc_jitter_max": self.zc_jitter_max, + "dshot_rx_good": self.dshot_rx_good, + "dshot_rx_bad": self.dshot_rx_bad, + "dshot_tx_frames": self.dshot_tx_frames, + "dshot_last_com_us": min(0xFFFF, comm_interval // 2) if self.rpm > 100 else 65535, + "dshot_telem_mode": 1, + "dshot_edt_mode": 0, + }) diff --git a/hwci/hwci/throttle/__init__.py b/hwci/hwci/throttle/__init__.py new file mode 100644 index 000000000..4177ddcfc --- /dev/null +++ b/hwci/hwci/throttle/__init__.py @@ -0,0 +1,5 @@ +"""Throttle-source backends (what physically commands the ESC signal wire).""" +from .base import ThrottleSource # noqa: F401 +from .null import NullThrottle # noqa: F401 +from .flightstand_src import FlightStandThrottle # noqa: F401 +from .external import ExternalSerialThrottle # noqa: F401 diff --git a/hwci/hwci/throttle/base.py b/hwci/hwci/throttle/base.py new file mode 100644 index 000000000..1e3fd115f --- /dev/null +++ b/hwci/hwci/throttle/base.py @@ -0,0 +1,48 @@ +"""Throttle-source abstraction. + +The throttle source is whatever generates the ESC signal during a test. Two +backends are provided: + +* :class:`~hwci.throttle.flightstand_src.FlightStandThrottle` - the Flight Stand + drives its own ESC output; simplest and perfectly synchronized with logging. +* :class:`~hwci.throttle.external.ExternalSerialThrottle` - a separate DShot/PWM + signal generator (e.g. an MCU bridge), for scripted DShot sequences or when + the stand can't emit the protocol you need. + +Throttle is always a normalized float in [0, 1]; each backend maps it to its +native units (PWM microseconds, DShot 48..2047, stand output units). +""" +from __future__ import annotations + +import abc + + +class ThrottleSource(abc.ABC): + @abc.abstractmethod + def arm(self) -> None: + """Bring the ESC to the armed/zero-throttle state.""" + + @abc.abstractmethod + def set(self, throttle: float) -> None: + """Command throttle in [0, 1].""" + + def quiesce(self) -> None: + """Make the signal line idle LOW so the AM32 bootloader will jump to + the app on the next ESC reset. Zero throttle is not enough for DShot + (frames keep the line high 40-75% of the time); backends that can + drop the signal entirely should override this.""" + self.set(0.0) + + def disarm(self) -> None: + self.set(0.0) + + def close(self) -> None: + pass + + def __enter__(self) -> "ThrottleSource": + self.arm() + return self + + def __exit__(self, *exc) -> None: + self.disarm() + self.close() diff --git a/hwci/hwci/throttle/external.py b/hwci/hwci/throttle/external.py new file mode 100644 index 000000000..ea0bdf085 --- /dev/null +++ b/hwci/hwci/throttle/external.py @@ -0,0 +1,54 @@ +"""External serial throttle source (DShot/PWM signal generator bridge). + +For runs where the ESC signal is produced by a dedicated generator rather than +the Flight Stand - e.g. a small MCU (Arduino/Teensy/STM32) running a DShot +generator, exposing a trivial serial protocol: + + ARM\\n -> arm / idle + T <0..2047>\\n -> set DShot throttle value + DISARM\\n + +This keeps scripted DShot300/600 demag-stress sequences possible even if the +stand only emits PWM. Swap in any generator by matching this line protocol, or +subclass and override :meth:`_write`. +""" +from __future__ import annotations + +from .base import ThrottleSource + +DSHOT_MIN = 48 +DSHOT_MAX = 2047 + + +class ExternalSerialThrottle(ThrottleSource): + def __init__(self, port: str, baud: int = 115200): + self.port = port + self.baud = baud + self._ser = None + + def _open(self): + import serial # only needed on the rig + if self._ser is None: + self._ser = serial.Serial(self.port, self.baud, timeout=0.5) + return self._ser + + def _write(self, line: str) -> None: + ser = self._open() + ser.write((line + "\n").encode()) + ser.flush() + + def arm(self) -> None: + self._write("ARM") + + def set(self, throttle: float) -> None: + throttle = max(0.0, min(1.0, throttle)) + value = int(DSHOT_MIN + throttle * (DSHOT_MAX - DSHOT_MIN)) + self._write(f"T {value}") + + def disarm(self) -> None: + self._write("DISARM") + + def close(self) -> None: + if self._ser is not None: + self._ser.close() + self._ser = None diff --git a/hwci/hwci/throttle/flightstand_src.py b/hwci/hwci/throttle/flightstand_src.py new file mode 100644 index 000000000..1d0725802 --- /dev/null +++ b/hwci/hwci/throttle/flightstand_src.py @@ -0,0 +1,27 @@ +"""Throttle source that uses the Flight Stand's own ESC output.""" +from __future__ import annotations + +import time + +from ..flightstand.base import ThrustStand +from .base import ThrottleSource + + +class FlightStandThrottle(ThrottleSource): + def __init__(self, stand: ThrustStand, arm_settle_s: float = 1.0): + self.stand = stand + self.arm_settle_s = arm_settle_s + + def arm(self) -> None: + self.stand.set_throttle(0.0) + # Hold zero throttle long enough for the ESC to arm. + time.sleep(self.arm_settle_s) + + def set(self, throttle: float) -> None: + self.stand.set_throttle(throttle) + + def quiesce(self) -> None: + # Deactivate the stand's ESC output entirely: the line is driven to + # logic 0, which is what lets the AM32 bootloader exit to the app + # (verified on the ARK 4IN1 bench; DShot-at-zero is NOT enough). + self.stand.deactivate() diff --git a/hwci/hwci/throttle/null.py b/hwci/hwci/throttle/null.py new file mode 100644 index 000000000..c7767546c --- /dev/null +++ b/hwci/hwci/throttle/null.py @@ -0,0 +1,24 @@ +"""No-op throttle: ESC signal is driven by something else (e.g. ARK FPV BDShot). + +Used by setup B (``throttle_backend: none``) when the harness only flashes +firmware and/or reads SWD while PX4 owns the signal wire. Must never be used +with Flight Stand throttle profiles — those require ``flightstand``. +""" +from __future__ import annotations + +from .base import ThrottleSource + + +class NullThrottle(ThrottleSource): + def arm(self) -> None: + pass + + def set(self, throttle: float) -> None: + pass + + def disarm(self) -> None: + pass + + def quiesce(self) -> None: + # Cannot drop the line; PX4 still owns the pin. + pass diff --git a/hwci/pyproject.toml b/hwci/pyproject.toml new file mode 100644 index 000000000..06479a942 --- /dev/null +++ b/hwci/pyproject.toml @@ -0,0 +1,31 @@ +[build-system] +requires = ["setuptools>=61"] +build-backend = "setuptools.build_meta" + +[project] +name = "am32-hwci" +version = "0.1.0" +description = "Hardware-in-the-loop CI harness for AM32 on the ARK 4IN1 ESC" +readme = "README.md" +requires-python = ">=3.10" +dependencies = [ + "pyelftools>=0.29", # read hwci_perf address + DWARF layout from the ELF + "pyserial>=3.5", # KISS telemetry + optional external throttle source + "PyYAML>=6.0", # test profiles / rig config + "numpy>=1.24", # metric math +] + +[project.optional-dependencies] +plot = ["matplotlib>=3.7"] # report plots +flightstand = ["grpcio>=1.51"] # real Flight Stand gRPC client +dev = ["pytest>=7.0", "grpcio-tools>=1.51"] + +[project.scripts] +hwci = "hwci.cli:main" + +[tool.setuptools.packages.find] +where = ["."] +include = ["hwci*"] + +[tool.pytest.ini_options] +testpaths = ["tests"] diff --git a/hwci/scripts/99-hwci.rules b/hwci/scripts/99-hwci.rules new file mode 100644 index 000000000..46329dcca --- /dev/null +++ b/hwci/scripts/99-hwci.rules @@ -0,0 +1,27 @@ +# udev rules for the AM32 hardware-CI bench. +# Install: sudo cp 99-hwci.rules /etc/udev/rules.d/ && sudo udevadm control --reload-rules && sudo udevadm trigger + +# --- ST-Link debug probes (V2 / V2.1 / V3) -> accessible without root --- +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3744", MODE="0660", TAG+="uaccess" +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3748", MODE="0660", TAG+="uaccess" +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374b", MODE="0660", TAG+="uaccess" +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374d", MODE="0660", TAG+="uaccess" +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374e", MODE="0660", TAG+="uaccess" +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374f", MODE="0660", TAG+="uaccess" +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3753", MODE="0660", TAG+="uaccess" +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3754", MODE="0660", TAG+="uaccess" + +# --- SEGGER J-Link (if used instead of ST-Link) --- +SUBSYSTEM=="usb", ATTRS{idVendor}=="1366", MODE="0660", TAG+="uaccess" + +# --- Stable serial-port names --------------------------------------------- +# Give the ESC telemetry adapter and (optional) external throttle generator +# stable /dev names so rig.yaml doesn't depend on enumeration order. +# Find your adapter's attributes with: +# udevadm info -a -n /dev/ttyUSB0 | grep -E 'idVendor|idProduct|serial' +# then fill in idVendor/idProduct and the unique serial below. +# +# Example (CP2102 with serial 0001): +# SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="0001", SYMLINK+="esc-telem" +# Example (FTDI with serial FT1ABCDE): +# SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="FT1ABCDE", SYMLINK+="esc-throttle" diff --git a/hwci/scripts/px4_bdshot_capture.py b/hwci/scripts/px4_bdshot_capture.py new file mode 100755 index 000000000..68429a2a1 --- /dev/null +++ b/hwci/scripts/px4_bdshot_capture.py @@ -0,0 +1,249 @@ +#!/usr/bin/env python3 +"""SETUP B only: capture PX4 ESC_STATUS over USB (BDShot eRPM / telem). + +Not for SETUP A (Flight Stand). See docs/BENCH_SETUPS.md. + +Intended setup (ARK FPV + PX4 BDShot): + * ARK FPV USB → host (/dev/ttyACM*) + * Actuator output = BDShot300/600 with EDT (if available) + * Optional: KISS/serial ESC telem on a UART (publishes into same esc_status) + * Optional second terminal: MAVLink shell (nsh) for `dshot` / `listener` + +Examples: + # Discover which ACM is PX4 + ./px4_bdshot_capture.py --port /dev/ttyACM0 --discover 3 + + # Log 30 s while you motor-test from QGC / nsh + ./px4_bdshot_capture.py --port /dev/ttyACM0 --duration 30 -o /tmp/bdshot_px4.csv + + # Request ESC_INFO once per second (serial telem / some ESCs) + ./px4_bdshot_capture.py --port /dev/ttyACM0 --esc-info --duration 20 + +CSV columns are host wall-clock + whatever ESC_STATUS / ESC_INFO fields +pymavlink exposes for this dialect. Correlate later with SWD `perf_dshot_*` +from a simultaneous hwci perf poll if the ST-Link is attached. +""" +from __future__ import annotations + +import argparse +import csv +import sys +import time +from pathlib import Path + + +def _connect(port: str, baud: int): + from pymavlink import mavutil + + # USB CDC: baud is ignored by the device but required by the API. + m = mavutil.mavlink_connection(port, baud=baud, source_system=255) + return m + + +def discover(port: str, baud: int, seconds: float) -> int: + m = _connect(port, baud) + t0 = time.time() + n = 0 + types: dict[str, int] = {} + print(f"listening on {port} for {seconds:.1f}s …", flush=True) + while time.time() - t0 < seconds: + msg = m.recv_match(blocking=True, timeout=0.25) + if msg is None: + continue + n += 1 + t = msg.get_type() + types[t] = types.get(t, 0) + 1 + if t == "HEARTBEAT": + print( + f" HEARTBEAT sys={msg.get_srcSystem()} " + f"comp={msg.get_srcComponent()} " + f"autopilot={getattr(msg, 'autopilot', '?')} " + f"type={getattr(msg, 'type', '?')}", + flush=True, + ) + print(f"messages={n} types={dict(sorted(types.items(), key=lambda kv: -kv[1])[:12])}") + return 0 if n else 1 + + +def _esc_status_rows(msg) -> list[dict]: + """Flatten ESC_STATUS into one row per reported ESC index.""" + rows = [] + # Dialect fields vary slightly; tolerate missing attrs. + count = int(getattr(msg, "count", 0) or 0) + # Some builds always fill 4/8 slots; use count when sane. + n = count if 0 < count <= 8 else 8 + t = time.time() + for i in range(n): + rpm = _arr(msg, "rpm", i) + volt = _arr(msg, "voltage", i) + cur = _arr(msg, "current", i) + temp = _arr(msg, "temperature", i) + # Skip completely empty slots + if rpm is None and volt is None and cur is None: + continue + if (rpm or 0) == 0 and (volt or 0) == 0 and (cur or 0) == 0 and i >= max(count, 1): + continue + rows.append({ + "t": round(t, 6), + "msg": "ESC_STATUS", + "index": i, + "count": count, + "rpm": rpm, + "voltage": volt, + "current": cur, + "temperature": temp, + "error_count": _arr(msg, "error_count", i), + }) + if not rows: + # Still record a heartbeat-like status sample so silence is visible + rows.append({ + "t": round(t, 6), + "msg": "ESC_STATUS", + "index": -1, + "count": count, + "rpm": None, + "voltage": None, + "current": None, + "temperature": None, + "error_count": None, + }) + return rows + + +def _arr(msg, name: str, i: int): + v = getattr(msg, name, None) + if v is None: + return None + if isinstance(v, (list, tuple)): + return v[i] if i < len(v) else None + return v + + +def _esc_info_row(msg) -> dict: + t = time.time() + return { + "t": round(t, 6), + "msg": "ESC_INFO", + "index": getattr(msg, "index", None), + "count": getattr(msg, "count", None), + "rpm": None, + "voltage": getattr(msg, "voltage", None), + "current": None, + "temperature": getattr(msg, "temperature", None), + "error_count": getattr(msg, "error_count", None), + "info": str(msg), + } + + +def capture(port: str, baud: int, duration: float, out: Path, + esc_info: bool, rate_hz: float) -> int: + m = _connect(port, baud) + print(f"waiting for HEARTBEAT on {port} …", flush=True) + m.wait_heartbeat(timeout=10) + print( + f"connected sys={m.target_system} comp={m.target_component}", + flush=True, + ) + + # Prefer onboard ESC telemetry stream if the dialect supports the request. + try: + m.mav.command_long_send( + m.target_system, m.target_component, + 511, # MAV_CMD_SET_MESSAGE_INTERVAL + 0, + 291, # ESC_STATUS (common dialect; ignore if unsupported) + 1e6 / max(rate_hz, 1.0), # us interval + 0, 0, 0, 0, 0, + ) + except Exception as e: + print(f"note: SET_MESSAGE_INTERVAL ESC_STATUS failed: {e}", flush=True) + + fields = [ + "t", "msg", "index", "count", "rpm", "voltage", "current", + "temperature", "error_count", "info", + ] + out.parent.mkdir(parents=True, exist_ok=True) + n_status = n_info = n_other = 0 + t0 = time.time() + next_info = t0 + with out.open("w", newline="") as fh: + w = csv.DictWriter(fh, fieldnames=fields, extrasaction="ignore") + w.writeheader() + print(f"logging → {out} for {duration:.1f}s (Ctrl-C to stop)", flush=True) + try: + while time.time() - t0 < duration: + now = time.time() + if esc_info and now >= next_info: + next_info = now + 1.0 + try: + # MAV_CMD_REQUEST_MESSAGE ESC_INFO (290) — best-effort + m.mav.command_long_send( + m.target_system, m.target_component, + 512, 0, 290, 0, 0, 0, 0, 0, 0, + ) + except Exception: + pass + msg = m.recv_match( + type=["ESC_STATUS", "ESC_INFO", "HIGHRES_IMU", "HEARTBEAT"], + blocking=True, timeout=0.2, + ) + if msg is None: + continue + t = msg.get_type() + if t == "ESC_STATUS": + for row in _esc_status_rows(msg): + w.writerow(row) + n_status += 1 + fh.flush() + elif t == "ESC_INFO": + w.writerow(_esc_info_row(msg)) + n_info += 1 + fh.flush() + elif t == "HEARTBEAT": + n_other += 1 + except KeyboardInterrupt: + print("\nstopped by user", flush=True) + + print( + f"done: ESC_STATUS samples={n_status} ESC_INFO={n_info} " + f"other={n_other} file={out}", + flush=True, + ) + if n_status == 0: + print( + "WARNING: no ESC_STATUS received.\n" + " - Confirm Actuators UI uses BDShot* (not plain DShot)\n" + " - Spin a motor (QGC Motor Test or nsh: dshot/actuator test)\n" + " - Serial telem UART configured if relying on KISS wire\n" + " - Try: mavlink shell → listener esc_status", + flush=True, + ) + return 2 + return 0 + + +def main(argv: list[str] | None = None) -> int: + ap = argparse.ArgumentParser(description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("--port", default="/dev/ttyACM0", + help="PX4 USB CDC port (default /dev/ttyACM0)") + ap.add_argument("--baud", type=int, default=115200) + ap.add_argument("--discover", type=float, default=0, + help="only listen N seconds and print message types") + ap.add_argument("--duration", type=float, default=30.0) + ap.add_argument("-o", "--output", type=Path, + default=Path("runs/bdshot_px4_capture.csv")) + ap.add_argument("--esc-info", action="store_true", + help="periodically request ESC_INFO") + ap.add_argument("--rate", type=float, default=50.0, + help="requested ESC_STATUS rate Hz (best-effort)") + args = ap.parse_args(argv) + + if args.discover: + return discover(args.port, args.baud, args.discover) + return capture(args.port, args.baud, args.duration, args.output, + args.esc_info, args.rate) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/hwci/scripts/px4_motor_stream.py b/hwci/scripts/px4_motor_stream.py new file mode 100755 index 000000000..19285302e --- /dev/null +++ b/hwci/scripts/px4_motor_stream.py @@ -0,0 +1,263 @@ +#!/usr/bin/env python3 +"""SETUP B only: drive motor via ARK FPV / PX4 ACTUATOR_TEST (BDShot). + +Not for SETUP A (Flight Stand throttle). Disconnect stand ESC output from +the signal pin before using this script. See docs/BENCH_SETUPS.md. + +Important behaviour (ARK FPV + PX4 1.18 + bench supply): + +* ``MAV_CMD_ACTUATOR_TEST`` (310) works; ``DO_MOTOR_TEST`` (209) is unsupported. +* Each test is only effective for ~3 s; if it expires the FC drops the output + **abruptly**. That can regenerate / spike and put a current-limited bench + supply into **fault mode**. Always re-fire before timeout and **ramp down**. +* Do **not** spam COMMAND_LONG at 10–50 Hz (servo looks live, motor dies). + Re-fire about every ``--refresh`` seconds (default 2.0) with ``--timeout 3``. +* Never jump high throttle → 0. Use a multi-step ramp-down at the end. + +Example: + + ./px4_motor_stream.py --port /dev/ttyACM2 \\ + --steps 0.15:6,0.30:6,0.50:6 --refresh 2.0 --timeout 3.0 \\ + --ramp-down 4 +""" +from __future__ import annotations + +import argparse +import csv +import sys +import threading +import time +from pathlib import Path + + +def main() -> int: + ap = argparse.ArgumentParser( + description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument('--port', default='/dev/ttyACM2') + ap.add_argument('--func', type=int, default=1, + help='ACTUATOR_OUTPUT_FUNCTION (Motor1=1)') + ap.add_argument('--steps', default='0.15:6,0.30:6,0.50:6', + help='comma list of value:seconds (no hard zero between)') + ap.add_argument('--refresh', type=float, default=1.5, + help='seconds between re-fire (must be well under ~3 s cap; ' + 'default 1.5 — late refresh lets PX4 hard-cut and can ' + 'fault the bench PSU)') + ap.add_argument('--timeout', type=float, default=3.0, + help='ACTUATOR_TEST timeout (PX4 ~3 s effective cap)') + ap.add_argument('--step-slew', type=float, default=1.0, + help='seconds to slew between consecutive step values') + ap.add_argument('--ramp-down', type=float, default=6.0, + help='seconds for final smooth ramp to zero (PSU-safe stop)') + ap.add_argument('--ramp-steps', type=int, default=12, + help='number of levels in final ramp-down') + ap.add_argument('-o', '--output', type=Path, default=None) + args = ap.parse_args() + + # Effective hold is ~2.8–3.0 s; refresh must leave margin or the FC drops + # the line hard (regen / current spike → PSU fault). + if args.refresh > args.timeout - 0.8: + print( + f'error: --refresh {args.refresh} too close to --timeout ' + f'{args.timeout}; use refresh <= timeout-0.8 (e.g. 1.5 / 3.0) ' + f'or the motor hard-stops and can fault the PSU', + file=sys.stderr) + return 2 + + from pymavlink import mavutil + + steps: list[tuple[float, float]] = [] + for part in args.steps.split(','): + part = part.strip() + if not part: + continue + v, s = part.split(':') + steps.append((float(v), float(s))) + if not steps: + print('error: no --steps', file=sys.stderr) + return 2 + + out = args.output or Path( + f"runs/bdshot_sustain_{time.strftime('%Y%m%d_%H%M%S')}.csv") + out.parent.mkdir(parents=True, exist_ok=True) + + m = mavutil.mavlink_connection(args.port, baud=115200, source_system=255) + print(f'waiting heartbeat on {args.port}…', flush=True) + m.wait_heartbeat(timeout=10) + sysid, comp = m.target_system, m.target_component + print(f'connected sys={sysid} comp={comp}', flush=True) + ml = mavutil.mavlink + + m.mav.command_long_send(sysid, comp, ml.MAV_CMD_SET_MESSAGE_INTERVAL, 0, + 291, 20000, 0, 0, 0, 0, 0) + m.mav.command_long_send(sysid, comp, ml.MAV_CMD_SET_MESSAGE_INTERVAL, 0, + 36, 50000, 0, 0, 0, 0, 0) + + stop = threading.Event() + rows: list[dict] = [] + lock = threading.Lock() + latest = {'rpm': 0, 'v': 0.0, 'i': 0.0, 'servo': None, 'cmd': 0.0} + current_cmd = 0.0 + + def reader() -> None: + while not stop.is_set(): + msg = m.recv_match( + type=['ESC_STATUS', 'SERVO_OUTPUT_RAW', 'STATUSTEXT'], + blocking=True, timeout=0.2) + if msg is None: + continue + t = time.time() + typ = msg.get_type() + if typ == 'STATUSTEXT': + print(f' TEXT: {msg.text}', flush=True) + elif typ == 'ESC_STATUS': + rpm = msg.rpm[0] if msg.rpm else 0 + v = msg.voltage[0] if msg.voltage else 0.0 + c = msg.current[0] if msg.current else 0.0 + latest.update(rpm=rpm, v=v, i=c) + with lock: + rows.append(dict( + t=t, msg='ESC_STATUS', rpm=rpm, voltage=v, current=c, + servo=str(latest.get('servo') or ''), cmd=latest['cmd'])) + elif typ == 'SERVO_OUTPUT_RAW': + s = [getattr(msg, f'servo{i}_raw', 0) for i in range(1, 5)] + latest['servo'] = s + with lock: + rows.append(dict( + t=t, msg='SERVO', rpm='', voltage='', current='', + servo=str(s), cmd=latest['cmd'])) + + threading.Thread(target=reader, daemon=True).start() + time.sleep(0.3) + + def fire(value: float, timeout: float | None = None) -> None: + nonlocal current_cmd + current_cmd = float(value) + latest['cmd'] = current_cmd + to = args.timeout if timeout is None else timeout + m.mav.command_long_send( + sysid, comp, ml.MAV_CMD_ACTUATOR_TEST, 0, + float(value), float(to), 0, 0, float(args.func), 0, 0) + + def sustain(value: float, seconds: float, label: str) -> None: + """Hold a value, re-firing before ACTUATOR_TEST expires (no hard stop).""" + print( + f'>>> SUSTAIN func={args.func} value={value:.3f} for {seconds:.1f}s ' + f'refresh={args.refresh}s ({label})', + flush=True) + t0 = time.time() + next_fire = t0 + last_print = t0 + while time.time() - t0 < seconds: + now = time.time() + if now >= next_fire: + fire(value) + next_fire = now + args.refresh + if now - last_print >= 1.0: + last_print = now + print( + f' t+{now - t0:4.1f}s cmd={value:.3f} rpm={latest["rpm"]} ' + f'V={latest["v"]:.2f} I={latest["i"]:.3f} ' + f'servo={latest.get("servo")}', + flush=True) + time.sleep(0.05) + + def slew(from_v: float, to_v: float, seconds: float, label: str) -> None: + """Linear slew; keeps re-firing so the timeout never hard-cuts mid-slew.""" + if seconds <= 0 or abs(to_v - from_v) < 1e-6: + return + n = max(2, int(seconds / max(args.refresh * 0.5, 0.25))) + print( + f'>>> SLEW {from_v:.3f} -> {to_v:.3f} over {seconds:.1f}s ' + f'({n} pts, {label})', + flush=True) + dt = seconds / n + for i in range(1, n + 1): + v = from_v + (to_v - from_v) * (i / n) + # hold each intermediate with continuous refresh for dt + sustain(v, dt, f'slew {i}/{n}') + + def ramp_down_safe(from_v: float) -> None: + """PSU-safe stop: multi-step descent, never jump to zero from high duty. + + Hard ACTUATOR_TEST expiry or cmd→0 from mid/high throttle has put the + 3 A bench supply into fault mode. Always descend gradually while still + refreshing so the ~3 s timeout never wins. + """ + if from_v <= 0.02: + # already essentially off — one gentle zero is OK + fire(0.0, timeout=1.0) + time.sleep(0.5) + return + print( + f'>>> RAMP-DOWN from {from_v:.3f} over {args.ramp_down:.1f}s ' + f'({args.ramp_steps} steps) — avoid PSU fault', + flush=True) + # Geometric-ish descent spends more time at low duty (less regen shock) + levels = [] + for i in range(1, args.ramp_steps + 1): + # ease-out: slow near zero + frac = 1.0 - (i / args.ramp_steps) ** 1.5 + levels.append(max(0.0, from_v * frac)) + levels[-1] = 0.0 + seg = args.ramp_down / len(levels) + prev = from_v + for v in levels: + # keep refreshing during each segment + sustain(v, max(seg, 0.35), f'ramp {v:.3f}') + prev = v + # linger at zero with refresh so we don't "expire" into a glitch + sustain(0.0, 1.0, 'park zero') + + try: + prev = 0.0 + for val, dur in steps: + if abs(val - prev) > 0.02 and args.step_slew > 0: + slew(prev, val, args.step_slew, 'step transition') + sustain(val, dur, f'throttle {val}') + prev = val + ramp_down_safe(prev) + except KeyboardInterrupt: + print('\ninterrupted — soft ramp-down', flush=True) + ramp_down_safe(current_cmd) + except Exception: + print('\nerror — soft ramp-down', flush=True) + try: + ramp_down_safe(current_cmd) + except Exception: + pass + raise + finally: + # only a soft zero if somehow still commanding; do not hard-cut from high + if current_cmd > 0.05: + try: + ramp_down_safe(current_cmd) + except Exception: + pass + else: + fire(0.0, timeout=0.3) + time.sleep(0.2) + stop.set() + time.sleep(0.2) + + fields = ['t', 'msg', 'rpm', 'voltage', 'current', 'servo', 'cmd'] + with out.open('w', newline='') as fh: + w = csv.DictWriter(fh, fieldnames=fields) + w.writeheader() + with lock: + w.writerows(rows) + + st = [r for r in rows if r['msg'] == 'ESC_STATUS'] + rpms = [float(r['rpm']) for r in st if r['rpm'] != ''] + print('==== SUMMARY ====', flush=True) + print(f'file={out} esc_status={len(st)}', flush=True) + if rpms: + print( + f'rpm min={min(rpms):.0f} max={max(rpms):.0f} ' + f'>1000={sum(1 for x in rpms if x > 1000)}/{len(rpms)}', + flush=True) + return 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/hwci/scripts/setup_ubuntu.sh b/hwci/scripts/setup_ubuntu.sh new file mode 100755 index 000000000..3eb990cdb --- /dev/null +++ b/hwci/scripts/setup_ubuntu.sh @@ -0,0 +1,51 @@ +#!/usr/bin/env bash +# Provision an Ubuntu 24.04 box as an AM32 hardware-CI bench / self-hosted runner. +# Idempotent: safe to re-run. Does NOT install the Flight Stand Software (vendor +# binary) or generate its gRPC stubs - see hwci/README.md for those steps. +set -euo pipefail + +HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +HWCI_DIR="$(dirname "$HERE")" +REPO_ROOT="$(dirname "$HWCI_DIR")" + +echo "==> apt packages" +sudo apt-get update +sudo apt-get install -y build-essential git python3-venv python3-pip openocd usbutils + +echo "==> ARM toolchain" +if ! ls "$REPO_ROOT"/tools/linux/*/bin/arm-none-eabi-gcc >/dev/null 2>&1 \ + && ! command -v arm-none-eabi-gcc >/dev/null 2>&1; then + ( cd "$REPO_ROOT" && make arm_sdk_install ) || sudo apt-get install -y gcc-arm-none-eabi +fi + +echo "==> udev rules (ST-Link + serial symlinks)" +sudo cp "$HERE/99-hwci.rules" /etc/udev/rules.d/99-hwci.rules +sudo udevadm control --reload-rules +sudo udevadm trigger + +echo "==> serial/usb group membership" +sudo usermod -aG dialout,plugdev "$USER" || true + +echo "==> python harness venv" +cd "$HWCI_DIR" +python3 -m venv .venv +# shellcheck disable=SC1091 +. .venv/bin/activate +pip install --upgrade pip +pip install -e '.[plot,flightstand,dev]' + +echo "==> self-test (simulator, no hardware needed)" +python -m hwci selftest + +cat <<'EOF' + +Done. Remaining manual steps (see hwci/README.md): + 1. Log out/in so dialout/plugdev group membership takes effect. + 2. Install the Tyto Flight Stand Software and generate its gRPC Python stubs. + 3. cp config/rig.example.yaml rig.yaml and edit for your wiring. + 4. Edit /etc/udev/rules.d/99-hwci.rules with your USB-serial serial numbers + so /dev/esc-telem and /dev/esc-throttle appear, then re-trigger udev. + 5. Capture a baseline: hwci ci --profile efficiency_sweep --config rig.yaml \ + --out runs/baseline && hwci baseline-save runs/baseline \ + --out baselines/ARK_4IN1_F051.json +EOF diff --git a/hwci/tests/conftest.py b/hwci/tests/conftest.py new file mode 100644 index 000000000..cf605a337 --- /dev/null +++ b/hwci/tests/conftest.py @@ -0,0 +1,80 @@ +"""Shared test fixtures. + +``host_perf_elf`` compiles the real firmware header (Inc/hwci_perf.h) with the +host C compiler into a small ELF carrying DWARF for ``hwci_perf_s`` and the +``hwci_perf`` symbol. The struct is naturally aligned with members <= 4 bytes, +so its layout is identical on the host and on the Cortex-M0 target. +""" +import shutil +import subprocess +from pathlib import Path + +import pytest + +REPO_ROOT = Path(__file__).resolve().parents[2] +HEADER_DIR = REPO_ROOT / "Inc" +_CC = shutil.which("cc") or shutil.which("gcc") + +_PROBE_C = """\ +#define HWCI_PERF 1 +#include "hwci_perf.h" +volatile hwci_perf_t hwci_perf = { + .magic = HWCI_PERF_MAGIC, .version = HWCI_PERF_VERSION, + .size = (uint16_t)sizeof(hwci_perf_t) }; +void hwci_perf_apply_cmd(void) {} +int main(void){ return (int)hwci_perf.size; } +""" + + +# Frozen copy of the v1 struct (pre zero-cross jitter block), so the host's +# backward-compat decode path is tested against a real compiled v1 ELF - the +# A side of an A/B bench session runs firmware with exactly this layout. +_PROBE_V1_C = """\ +#include +typedef struct hwci_perf_s { + uint32_t magic; uint16_t version; uint16_t size; + uint16_t ctrl_exec_us_last; uint16_t ctrl_exec_us_max; + uint16_t ctrl_period_us_last; uint16_t ctrl_period_us_max; + uint16_t ctrl_period_us_min; + uint16_t main_loop_us_last; uint16_t main_loop_us_max; + uint16_t input; uint16_t duty_cycle; uint16_t e_rpm; + uint16_t voltage_cv; int16_t current_ca; int16_t temperature_c; + uint8_t bemf_timeout_state; uint8_t armed; uint8_t running; + uint8_t _pad0; uint16_t _pad1; + uint32_t loop_iters; uint32_t zero_cross_count; + uint32_t commutation_interval; uint32_t commutation_interval_max; + uint32_t update_count; volatile uint32_t host_cmd; +} hwci_perf_t; +volatile hwci_perf_t hwci_perf = { + .magic = 0x31435748u, .version = 1, .size = (uint16_t)sizeof(hwci_perf_t) }; +int main(void){ return (int)hwci_perf.size; } +""" + + +def _compile_probe(tmp_path_factory, name: str, source: str, + include_dir=None) -> str: + pytest.importorskip("elftools") + if _CC is None: + pytest.skip("no host C compiler available") + d = tmp_path_factory.mktemp(name) + src = d / "probe.c" + src.write_text(source) + out = d / "probe.elf" + cmd = [_CC, "-g", "-O0"] + if include_dir is not None: + cmd.append(f"-I{include_dir}") + subprocess.run(cmd + [str(src), "-o", str(out)], + check=True, capture_output=True) + return str(out) + + +@pytest.fixture(scope="session") +def host_perf_elf(tmp_path_factory): + if not (HEADER_DIR / "hwci_perf.h").exists(): + pytest.skip("Inc/hwci_perf.h not found") + return _compile_probe(tmp_path_factory, "perf_elf", _PROBE_C, HEADER_DIR) + + +@pytest.fixture(scope="session") +def host_perf_elf_v1(tmp_path_factory): + return _compile_probe(tmp_path_factory, "perf_elf_v1", _PROBE_V1_C) diff --git a/hwci/tests/test_app_alive.py b/hwci/tests/test_app_alive.py new file mode 100644 index 000000000..20ab187ae --- /dev/null +++ b/hwci/tests/test_app_alive.py @@ -0,0 +1,88 @@ +"""_ensure_app_alive: bootloader-stuck ESC recovery on live-source bring-up. + +On this rig the stand's inactive ESC output leaves the signal line high, and +the AM32 bootloader then never jumps to the app after a flash/power-cycle +(observed on the ARK 4IN1 bench: perf magic reads 0x00000000, PC loops in the +bootloader). The runner must drive zero throttle, reset, and wait for the +app's magic instead of handing a dead perf channel to the run. +""" +from __future__ import annotations + +import pytest + +from hwci.perf import PerfDecodeError +from hwci.runner import _ensure_app_alive + + +class FakeReader: + """perf_reader.read() fails until the fake target has been reset.""" + + def __init__(self, alive_after_resets: int): + self.alive_after_resets = alive_after_resets + self.resets = 0 + + def read(self): + if self.resets >= self.alive_after_resets: + return object() + raise PerfDecodeError("bad magic 0x00000000") + + +class FakeDbg: + def __init__(self, reader: FakeReader): + self._reader = reader + + def reset_run(self): + self._reader.resets += 1 + + +class FakeThrottle: + def __init__(self): + self.commands = [] + + def set(self, throttle): + self.commands.append(throttle) + + def quiesce(self): + self.commands.append("quiesce") + + +def test_already_alive_touches_nothing(): + reader = FakeReader(alive_after_resets=0) + dbg, throttle = FakeDbg(reader), FakeThrottle() + _ensure_app_alive(dbg, reader, throttle) + assert reader.resets == 0 + assert throttle.commands == [] + + +def test_stuck_in_bootloader_recovers_via_reset(): + reader = FakeReader(alive_after_resets=1) + dbg, throttle = FakeDbg(reader), FakeThrottle() + _ensure_app_alive(dbg, reader, throttle) + assert reader.resets == 1 + # the signal must be DROPPED (not DShot-at-zero) before the reset so the + # line is driven low and the bootloader jumps to the app + assert throttle.commands == ["quiesce"] + + +def test_never_alive_raises_actionable_error(monkeypatch): + # collapse the wait loops so the failure path is fast + import hwci.runner as runner + monkeypatch.setattr(runner.time, "sleep", lambda s: None) + clock = iter(range(0, 10_000)) + monkeypatch.setattr(runner.time, "monotonic", lambda: float(next(clock))) + + reader = FakeReader(alive_after_resets=99) + dbg, throttle = FakeDbg(reader), FakeThrottle() + with pytest.raises(RuntimeError, match="bootloader|HWCI_PERF"): + _ensure_app_alive(dbg, reader, throttle) + assert reader.resets == 2 # both attempts exhausted + + +def test_debugger_error_propagates(): + class DeadProbeReader: + def read(self): + raise ConnectionError("SWD gone") + + reader = DeadProbeReader() + with pytest.raises(ConnectionError): + _ensure_app_alive(FakeDbg(FakeReader(0)), reader, FakeThrottle()) diff --git a/hwci/tests/test_baseline.py b/hwci/tests/test_baseline.py new file mode 100644 index 000000000..813e57ece --- /dev/null +++ b/hwci/tests/test_baseline.py @@ -0,0 +1,213 @@ +"""Tests for baseline save/compare regression gating.""" +import copy + +from hwci import baseline as bl +from hwci import metrics as metricsmod +from hwci.config import RigConfig, load_profile +from hwci.runner import build_sim_sources, run_profile + + +def _metrics(profile_name="ci_smoke"): + rig = RigConfig() + profile = load_profile(profile_name) + sources = build_sim_sources(rig, profile) + try: + result = run_profile(profile, sources, realtime=False, meta={}) + finally: + sources.close() + return metricsmod.compute(result, profile) + + +def test_identical_metrics_pass(): + m = _metrics() + base = {"metrics": m} + result = bl.compare(m, base) + assert result["passed"] + + +def test_efficiency_regression_fails(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + # 30% drop is well past the 15% gate (widened from an initial 3% guess + # after a same-firmware repeatability run showed up to 8.7% swing at + # high power on the physical bench - see Thresholds.efficiency_drop_pct). + m["summary"]["peak_efficiency_gf_per_w"] *= 0.70 + for p in m["steady_points"]: + p["eff_gf_per_w"] *= 0.70 + result = bl.compare(m, base) + assert not result["passed"] + assert any(c["name"] == "peak_efficiency_gf_per_w" and not c["pass"] + for c in result["checks"]) + + +def test_loop_time_regression_fails(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + # Well past both the +15% and the +45us absolute slack. + m["summary"]["worst_ctrl_exec_us_steady"] = ( + base["metrics"]["summary"]["worst_ctrl_exec_us_steady"] + 100) + result = bl.compare(m, base) + assert not result["passed"] + + +def test_loop_time_gate_prefers_steady_key(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + # A start-transient spike in the raw run-max must NOT fail the gate when + # the steady-window value is unchanged (observed 705 vs 950us run-to-run). + m["summary"]["worst_ctrl_exec_us"] = 950 + result = bl.compare(m, base) + assert result["passed"] + + +def test_loop_time_gate_falls_back_without_steady_key(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + del base["metrics"]["summary"]["worst_ctrl_exec_us_steady"] + m["summary"]["worst_ctrl_exec_us"] = ( + base["metrics"]["summary"]["worst_ctrl_exec_us"] + 100) + result = bl.compare(m, base) + assert not result["passed"] + + +def test_loop_time_equality_passes(): + # A baseline must pass against itself even with large absolute values + # (950us start transients failed the old <=45us absolute-cap semantics). + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + for k in ("worst_ctrl_exec_us", "worst_ctrl_exec_us_steady"): + base["metrics"]["summary"][k] = 950 + m["summary"][k] = 950 + assert bl.compare(m, base)["passed"] + + +def test_negative_efficiency_equality_passes(): + # Naive percent math fails an identical negative value against itself. + assert bl._worse_is_lower(-1.0, -1.0, 3.0) + assert not bl._worse_is_lower(-1.0, -1.06, 3.0) # real 6% worsening fails + assert bl._worse_is_lower(9.0, 8.8, 3.0) # positive within 3% + + +def test_low_power_point_not_gated_even_with_prop(): + # A real prop can still produce a low-power point (e.g. 10% throttle) + # where thrust/power is a ratio of two small noisy numbers - observed on + # the bench: 2.3W swung -73% efficiency run-to-run on unchanged hardware. + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + base["metrics"]["steady_points"][0]["elec_power_w"] = 2.3 + base["metrics"]["steady_points"][0]["eff_gf_per_w"] = 7.72 + m["steady_points"][0]["elec_power_w"] = 2.3 + m["steady_points"][0]["eff_gf_per_w"] = 2.05 # -73%, would fail the % gate + result = bl.compare(m, base) + label = base["metrics"]["steady_points"][0]["segment"] + eff_check = next(c for c in result["checks"] if c["name"] == f"eff@{label}") + assert eff_check["pass"] + assert "not gated" in eff_check["note"] + + +def test_peak_efficiency_excludes_low_power_points(): + # "Peak" is a max() over throttle points, which amplifies whichever point + # is noisiest. If the literal peak sits at a low-power point, the GATE + # must recompute peak from points above the power floor instead of + # trusting the summary scalar - else it gates pure noise every run. + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + pts_b = base["metrics"]["steady_points"] + pts_c = m["steady_points"] + assert len(pts_b) >= 2 + # Low-power point has the highest g/W in both runs (as observed on the + # bench) but swings wildly; a real, well-powered point stays stable. + pts_b[0]["elec_power_w"], pts_b[0]["eff_gf_per_w"] = 2.3, 50.0 + pts_c[0]["elec_power_w"], pts_c[0]["eff_gf_per_w"] = 2.3, 5.0 + pts_b[1]["elec_power_w"], pts_b[1]["eff_gf_per_w"] = 100.0, 1.0 + pts_c[1]["elec_power_w"], pts_c[1]["eff_gf_per_w"] = 100.0, 1.0 + result = bl.compare(m, base) + peak_check = next(c for c in result["checks"] + if c["name"] == "peak_efficiency_gf_per_w") + assert peak_check["baseline"] == 1.0 # not 50.0 - the noisy point excluded + assert peak_check["current"] == 1.0 + assert peak_check["pass"] + + +def test_noprop_efficiency_noise_not_gated(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + # No-prop rig: baseline g/W is noise around zero; must not gate at all. + base["metrics"]["summary"]["peak_efficiency_gf_per_w"] = -0.218 + m["summary"]["peak_efficiency_gf_per_w"] = 0.3 # different noise, still ok + for p in base["metrics"]["steady_points"]: + p["eff_gf_per_w"] = -0.1 + for p in m["steady_points"]: + p["eff_gf_per_w"] = 0.2 + result = bl.compare(m, base) + assert result["passed"] + assert any("not gated" in c["note"] for c in result["checks"]) + + +def test_new_demag_fails(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + base["metrics"]["summary"]["demag_events"] = 0 + m["summary"]["demag_events"] = 2 + result = bl.compare(m, base) + assert not result["passed"] + + +def test_save_and_load(tmp_path): + m = _metrics() + path = bl.save_baseline(m, tmp_path / "base.json", meta={"target": "X"}) + loaded = bl.load_baseline(path) + assert loaded["format_version"] == bl.FORMAT_VERSION + assert loaded["metrics"]["summary"]["max_thrust_gf"] == m["summary"]["max_thrust_gf"] + assert loaded["meta"]["target"] == "X" + + +# --- fail-closed behaviour -------------------------------------------------- + +def test_missing_current_metric_fails(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + m["summary"]["worst_ctrl_exec_us_steady"] = None # dead perf channel + result = bl.compare(m, base) + assert not result["passed"] + assert any(c["name"] == "worst_ctrl_exec_us_steady" and not c["pass"] + for c in result["checks"]) + + +def test_nan_current_metric_fails(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + m["summary"]["max_cpu_load_pct"] = float("nan") + result = bl.compare(m, base) + assert not result["passed"] + + +def test_dead_channel_coverage_fails(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + # Perf channel died mid-run: only a handful of samples made it. + m["summary"]["perf_sample_count"] = 3 + result = bl.compare(m, base) + assert any(c["name"] == "perf_coverage" and not c["pass"] + for c in result["checks"]) + assert not result["passed"] + + +def test_identity_mismatch_fails(): + m = _metrics() + base = {"meta": {"target": "OTHER_TARGET", "profile": "ci_smoke"}, + "metrics": copy.deepcopy(m)} + result = bl.compare(m, base, + current_meta={"target": "ARK_4IN1_F051", + "profile": "ci_smoke"}) + assert not result["passed"] + assert any(c["name"] == "baseline_target" and not c["pass"] + for c in result["checks"]) + + +def test_missing_steady_segment_fails(): + m = _metrics() + base = {"metrics": copy.deepcopy(m)} + m["steady_points"] = m["steady_points"][:-1] # a segment produced no data + result = bl.compare(m, base) + assert not result["passed"] diff --git a/hwci/tests/test_battery.py b/hwci/tests/test_battery.py new file mode 100644 index 000000000..df73b5e2c --- /dev/null +++ b/hwci/tests/test_battery.py @@ -0,0 +1,73 @@ +"""Pre-flight battery check: refuse to arm a test on a pack that's already +too low for its declared cell count (see check_battery/_live_voltage in +hwci/runner.py, wired into build_live_sources).""" +import pytest + +from hwci.perf import PerfSample +from hwci.runner import (DEFAULT_MIN_CELL_VOLTAGE, BatteryTooLowError, + _live_voltage, check_battery) + + +class _FakeStand: + def __init__(self, voltage_v): + self._voltage_v = voltage_v + + def read_sample(self): + return type("Sample", (), {"voltage_v": self._voltage_v})() + + +def _boom(): + raise RuntimeError("SWD gone") + + +# -------------------------------------------------------------------------- +# check_battery: pure threshold logic +# -------------------------------------------------------------------------- +def test_healthy_6s_pack_passes(): + check_battery(24.9, battery_cells=6) # 4.15 V/cell - matches bench baseline + + +def test_6s_pack_below_default_cutoff_raises(): + with pytest.raises(BatteryTooLowError, match="6S"): + check_battery(18.5, battery_cells=6) # 3.08 V/cell < 3.3 V/cell default + + +def test_error_message_names_actual_and_minimum_voltage(): + with pytest.raises(BatteryTooLowError) as exc_info: + check_battery(18.5, battery_cells=6) + msg = str(exc_info.value) + assert "18.50" in msg + assert f"{6 * DEFAULT_MIN_CELL_VOLTAGE:.2f}" in msg + + +def test_unverifiable_voltage_fails_closed(): + with pytest.raises(BatteryTooLowError, match="cannot verify"): + check_battery(None, battery_cells=6) + + +def test_custom_cutoff_overrides_default(): + check_battery(19.0, battery_cells=6, min_cell_voltage=3.0) # 3.17 V/cell, above 3.0 floor + with pytest.raises(BatteryTooLowError): + check_battery(19.0, battery_cells=6, min_cell_voltage=3.3) # same pack, stricter floor + + +def test_boundary_voltage_is_not_too_low(): + # exactly at the minimum should pass (strict "<" in the implementation) + check_battery(6 * DEFAULT_MIN_CELL_VOLTAGE, battery_cells=6) + + +# -------------------------------------------------------------------------- +# _live_voltage: stand preferred, perf struct as fallback +# -------------------------------------------------------------------------- +def test_live_voltage_reads_stand_when_present(): + stand = _FakeStand(voltage_v=22.2) + assert _live_voltage(stand, perf_source=_boom) == 22.2 # perf never consulted + + +def test_live_voltage_falls_back_to_perf_without_a_stand(): + pf = PerfSample(raw={"voltage_cv": 2210}) + assert _live_voltage(None, perf_source=lambda: pf) == pytest.approx(22.10) + + +def test_live_voltage_none_when_no_stand_and_perf_unreadable(): + assert _live_voltage(None, perf_source=_boom) is None diff --git a/hwci/tests/test_cli.py b/hwci/tests/test_cli.py new file mode 100644 index 000000000..147d32fc4 --- /dev/null +++ b/hwci/tests/test_cli.py @@ -0,0 +1,82 @@ +"""CLI-layer tests: sim/hw mode selection, baseline bootstrap, self-describing runs.""" +import argparse + +from hwci import cli +from hwci.config import RigConfig, load_profile +from hwci.model import RunResult +from hwci.runner import DEFAULT_MIN_CELL_VOLTAGE + + +def _ns(**kw): + return argparse.Namespace(**kw) + + +def test_sim_requires_explicit_flag_or_no_config(): + assert cli._use_sim(_ns(sim=True, config="rig.yaml")) + assert cli._use_sim(_ns(sim=False, config=None)) + # A rig config without --sim is ALWAYS a hardware run - a sim backend + # value can no longer flip it (load_rig rejects sim backends anyway). + assert not cli._use_sim(_ns(sim=False, config="rig.yaml")) + + +def test_missing_baseline_warns_and_skips(tmp_path, capsys): + assert cli._load_baseline(str(tmp_path / "nope.json")) is None + assert "not found" in capsys.readouterr().err + assert cli._load_baseline(None) is None + + +def test_profile_for_prefers_embedded_definition(): + profile = load_profile("ci_smoke") + edited = cli.profile_to_dict(profile) + edited["demag_rpm_drop_fraction"] = 0.11 # differs from today's YAML + result = RunResult(meta={"profile": "ci_smoke", "profile_def": edited}) + assert cli._profile_for(result).demag_rpm_drop_fraction == 0.11 + # No embedded definition -> falls back to loading by name. + legacy = RunResult(meta={"profile": "ci_smoke"}) + assert cli._profile_for(legacy).name == "ci_smoke" + + +def test_selftest_runs_clean(capsys): + rc = cli.cmd_selftest(_ns(profile="ci_smoke")) + assert rc == 0 + out = capsys.readouterr().out + assert "steady points" in out + + +def test_run_and_ci_expose_battery_flags_with_defaults(): + parser = cli.build_parser() + run_ns = parser.parse_args(["run", "--profile", "ci_smoke"]) + assert run_ns.battery_cells is None + assert run_ns.min_cell_voltage == DEFAULT_MIN_CELL_VOLTAGE + + ci_ns = parser.parse_args(["ci", "--battery-cells", "6", + "--min-cell-voltage", "3.5"]) + assert ci_ns.battery_cells == 6 + assert ci_ns.min_cell_voltage == 3.5 + + +def test_run_and_ci_expose_no_tare_flag(): + parser = cli.build_parser() + assert parser.parse_args(["run", "--profile", "ci_smoke"]).no_tare is False + assert parser.parse_args(["ci", "--no-tare"]).no_tare is True + + +def test_sim_runs_are_never_marked_tared(): + # The simulator has no load cells to zero; meta must say so even when + # taring wasn't explicitly disabled, or a sim run dir would claim a + # pre-flight step that never happened. + rig = RigConfig() + profile = load_profile("ci_smoke") + result = cli._execute(rig, profile, sim=True) + assert result.meta["tared"] is False + + +def test_battery_check_does_not_apply_in_sim_mode(): + # The built-in simulator's nominal pack voltage doesn't represent any + # particular real cell count, so --battery-cells must be a no-op under + # --sim rather than spuriously aborting every simulated/offline run. + rig = RigConfig() + profile = load_profile("ci_smoke") + result = cli._execute(rig, profile, sim=True, battery_cells=6) + assert result.meta["aborted"] is None + assert result.meta["battery_cells"] == 6 # still recorded for the record diff --git a/hwci/tests/test_config.py b/hwci/tests/test_config.py new file mode 100644 index 000000000..e7a5a7820 --- /dev/null +++ b/hwci/tests/test_config.py @@ -0,0 +1,76 @@ +"""Tests for strict rig-config validation and profile (de)serialization.""" +import pytest + +from hwci.config import (RigConfig, load_profile, load_rig, + profile_from_dict, profile_to_dict) + +VALID_RIG = """\ +target: ARK_4IN1_F051 +debugger_backend: openocd +telem_backend: serial +throttle_backend: flightstand +stand_backend: grpc +pole_pairs: 11 +""" + + +def _write(tmp_path, text): + p = tmp_path / "rig.yaml" + p.write_text(text) + return str(p) + + +def test_valid_rig_loads(tmp_path): + rig = load_rig(_write(tmp_path, VALID_RIG)) + assert rig.debugger_backend == "openocd" + assert rig.pole_pairs == 11 + + +def test_unknown_key_rejected(tmp_path): + with pytest.raises(ValueError, match="unknown key"): + load_rig(_write(tmp_path, VALID_RIG + "debugger_bakend: openocd\n")) + + +def test_unknown_backend_value_rejected(tmp_path): + bad = VALID_RIG.replace("stand_backend: grpc", "stand_backend: gprc") + with pytest.raises(ValueError, match="stand_backend"): + load_rig(_write(tmp_path, bad)) + + +def test_sim_backend_rejected_in_rig_file(tmp_path): + bad = VALID_RIG.replace("stand_backend: grpc", "stand_backend: sim") + with pytest.raises(ValueError, match="not allowed in a rig file"): + load_rig(_write(tmp_path, bad)) + + +def test_omitted_backend_rejected_in_rig_file(tmp_path): + # An omitted backend would default to "sim" - a rig file must be explicit. + partial = VALID_RIG.replace("telem_backend: serial\n", "") + with pytest.raises(ValueError, match="telem_backend"): + load_rig(_write(tmp_path, partial)) + + +def test_flightstand_throttle_needs_a_stand(tmp_path): + bad = VALID_RIG.replace("stand_backend: grpc", "stand_backend: none") + with pytest.raises(ValueError, match="flightstand"): + load_rig(_write(tmp_path, bad)) + + +def test_none_backends_allowed(tmp_path): + text = VALID_RIG.replace("stand_backend: grpc", "stand_backend: none") + text = text.replace("throttle_backend: flightstand", + "throttle_backend: external") + rig = load_rig(_write(tmp_path, text)) + assert rig.stand_backend == "none" + + +def test_no_config_gives_sim_defaults(): + rig = load_rig(None) + assert rig.stand_backend == "sim" + rig.validate() # sim allowed for the built-in default + + +def test_profile_roundtrips_through_dict(): + p = load_profile("demag_step_stress") + q = profile_from_dict(profile_to_dict(p)) + assert q == p diff --git a/hwci/tests/test_elf.py b/hwci/tests/test_elf.py new file mode 100644 index 000000000..88e025b66 --- /dev/null +++ b/hwci/tests/test_elf.py @@ -0,0 +1,35 @@ +"""Tests for ELF symbol + DWARF struct-layout extraction.""" +import struct + +import pytest + +pytest.importorskip("elftools") + +from hwci import elf, perf # noqa: E402 + + +def test_find_symbol(host_perf_elf): + sym = elf.find_symbol(host_perf_elf, "hwci_perf") + assert sym.size == perf.SIZE == 96 + + +def test_dwarf_layout_matches_canonical(host_perf_elf): + members = {m.name: m for m in elf.struct_layout(host_perf_elf, "hwci_perf_s")} + offset = 0 + for name, code in perf.FIELDS: + size = struct.calcsize(code) + if not name.startswith("_pad"): + assert name in members, f"{name} missing from DWARF" + assert members[name].offset == offset, ( + f"{name}: DWARF off {members[name].offset} != canonical {offset}") + assert members[name].size == size, ( + f"{name}: DWARF size {members[name].size} != canonical {size}") + assert members[name].signed == (code in ("b", "h", "i")), ( + f"{name}: signedness mismatch") + offset += size + assert offset == perf.SIZE + + +def test_missing_symbol_raises(host_perf_elf): + with pytest.raises(elf.ElfError): + elf.find_symbol(host_perf_elf, "definitely_not_a_symbol") diff --git a/hwci/tests/test_flightstand_sim.py b/hwci/tests/test_flightstand_sim.py new file mode 100644 index 000000000..579e21b51 --- /dev/null +++ b/hwci/tests/test_flightstand_sim.py @@ -0,0 +1,48 @@ +"""Tests for the simulated Flight Stand backend.""" +import pytest + +from hwci.flightstand.base import SafetyLimits +from hwci.flightstand.simulator import SimulatedStand, StandSafetyTripped +from hwci.sim import MotorParams, RigSimulator + + +def _stand(**kw): + # Deterministic: fixed timestep, no noise. + rig = RigSimulator(params=kw.pop("params", MotorParams()), noise=0.0) + return SimulatedStand(rig, fixed_dt=0.01).open() + + +def test_throttle_produces_thrust(): + stand = _stand() + stand.set_throttle(0.7) + last = None + for _ in range(200): + last = stand.read_sample() + assert last.thrust_n > 0 + assert last.rpm > 1000 + assert last.efficiency_gf_per_w > 0 + + +def test_zero_throttle_zero_thrust(): + stand = _stand() + stand.set_throttle(0.0) + for _ in range(50): + s = stand.read_sample() + assert s.thrust_n == pytest.approx(0.0, abs=1e-6) + + +def test_safety_limit_trips(): + stand = _stand() + stand.set_safety_limits(SafetyLimits(max_current_a=5.0)) + stand.set_throttle(1.0) + with pytest.raises(StandSafetyTripped): + for _ in range(500): + stand.read_sample() + + +def test_context_manager_zeroes_on_exit(): + rig = RigSimulator(noise=0.0) + with SimulatedStand(rig, fixed_dt=0.01) as stand: + stand.set_throttle(0.5) + stand.read_sample() + assert stand._throttle == 0.0 diff --git a/hwci/tests/test_kiss.py b/hwci/tests/test_kiss.py new file mode 100644 index 000000000..b223da655 --- /dev/null +++ b/hwci/tests/test_kiss.py @@ -0,0 +1,81 @@ +"""Tests for the KISS ESC telemetry decoder.""" +import struct + +from hwci.esc_telem import crc8, encode_frame, parse_frame +from hwci.esc_telem.kiss import KissStream + + +def _make_frame(temp, volt_cv, cur_ca, cons, erpm100): + body = struct.pack(">bHHHH", temp, volt_cv, cur_ca, cons, erpm100) + return body + bytes([crc8(body)]) + + +def test_crc8_known_vector(): + # CRC of an all-zero 9-byte body is 0 for this polynomial/init. + assert crc8(b"\x00" * 9) == 0 + + +def test_crc8_table_matches_bitwise_reference(): + def crc8_bitwise(data): + crc = 0 + for byte in data: + crc ^= byte + for _ in range(8): + crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF + return crc + for vec in (b"\x01", b"\xff" * 9, bytes(range(9)), b"\xa5\x5a\x00\x42"): + assert crc8(vec) == crc8_bitwise(vec) + + +def test_encode_parse_roundtrip(): + blob = encode_frame(temperature_c=42, voltage_cv=1650, current_ca=1234, + consumption_mah=56, erpm100=200) + f = parse_frame(blob) + assert f.crc_ok + assert (f.temperature_c, f.voltage_v, f.current_a, + f.consumption_mah, f.e_rpm) == (42, 16.5, 12.34, 56, 20000) + + +def test_parse_valid_frame(): + frame = _make_frame(temp=42, volt_cv=1650, cur_ca=1234, cons=56, erpm100=200) + f = parse_frame(frame) + assert f.crc_ok + assert f.temperature_c == 42 + assert f.voltage_v == 1650 / 100.0 + assert f.current_a == 1234 / 100.0 + assert f.consumption_mah == 56 + assert f.e_rpm == 20000 + + +def test_bad_crc_flagged(): + frame = bytearray(_make_frame(20, 1600, 100, 1, 100)) + frame[-1] ^= 0xFF + f = parse_frame(bytes(frame)) + assert not f.crc_ok + + +def test_stream_resync_after_garbage(): + good = _make_frame(25, 1600, 500, 10, 150) + stream = KissStream() + # Prepend 3 junk bytes; the framer must slide past them and recover. + frames = list(stream.feed(b"\x11\x22\x33" + good)) + assert len(frames) == 1 + assert frames[0].temperature_c == 25 + assert frames[0].e_rpm == 15000 + + +def test_stream_multiple_frames(): + f1 = _make_frame(10, 1500, 100, 1, 50) + f2 = _make_frame(11, 1510, 110, 2, 60) + stream = KissStream() + out = list(stream.feed(f1 + f2)) + assert [f.temperature_c for f in out] == [10, 11] + + +def test_stream_partial_then_complete(): + f = _make_frame(30, 1700, 800, 20, 250) + stream = KissStream() + assert list(stream.feed(f[:6])) == [] # nothing yet + out = list(stream.feed(f[6:])) + assert len(out) == 1 + assert out[0].e_rpm == 25000 diff --git a/hwci/tests/test_metrics.py b/hwci/tests/test_metrics.py new file mode 100644 index 000000000..a3e4977eb --- /dev/null +++ b/hwci/tests/test_metrics.py @@ -0,0 +1,201 @@ +"""Metric-math tests on synthetic rows: demag channels and CPU-load robustness.""" +from hwci import metrics as metricsmod +from hwci.config import Profile, Segment +from hwci.model import RunResult + + +def _profile(): + return Profile(name="synthetic", sample_rate_hz=100.0, + segments=[Segment(label="hold", throttle=0.9, + duration_s=1.0, steady=True)]) + + +def _rows(n, **cols): + """n rows at 100 Hz; cols maps column -> list or callable(i).""" + rows = [] + for i in range(n): + row = {"t": i * 0.01, "segment": "hold", "throttle_cmd": 0.9} + for k, v in cols.items(): + row[k] = v(i) if callable(v) else v[i] + rows.append(row) + return rows + + +def test_bemf_counter_values_all_count(): + # Firmware bemf_timeout_happened is a counter (2, 3, ... latched at 102), + # not a boolean - every non-zero sample is a timeout state. + vals = [0, 0, 1, 2, 50, 102, 0, 0, 0, 0] + rows = _rows(len(vals), perf_bemf_timeout=vals) + d = metricsmod.detect_demag(RunResult(rows=rows), _profile()) + assert d["bemf_timeout_samples"] == 4 + assert d["event_count"] >= 1 + + +def test_rpm_collapse_detected_at_high_throttle(): + # Steady 90% throttle; RPM collapses 40% mid-segment (desync whose + # transient firmware flags fell between SWD samples). + rpm = [20000.0] * 30 + [11000.0] * 10 + [20000.0] * 60 + rows = _rows(len(rpm), stand_rpm=rpm) + d = metricsmod.detect_demag(RunResult(rows=rows), _profile()) + assert d["rpm_drop_samples"] >= 5 + assert d["event_count"] >= 1 + + +def test_spoolup_is_not_a_collapse(): + # RPM rising monotonically after a step must not flag (the reference is + # the running max, which trails a rising signal). + rpm = [5000.0 + 150.0 * i for i in range(100)] + rows = _rows(len(rpm), stand_rpm=rpm) + d = metricsmod.detect_demag(RunResult(rows=rows), _profile()) + assert d["rpm_drop_samples"] == 0 + + +def test_smooth_rampdown_is_not_a_collapse(): + # A multi-second ramp-down moves throttle by a tiny amount each 10ms + # tick; RPM falling in lockstep with a commanded ramp-down is expected + # deceleration, not desync (observed false positive on the bench: a + # efficiency_sweep 100%->0% rampdn flagged with zero bemf timeouts, + # commutation spikes, or eRPM mismatch). + n = 400 # 4s at 100Hz + throttle = [1.0 - 1.0 * i / (n - 1) for i in range(n)] # smooth 1.0 -> 0.0 + rpm = [28000.0 * t for t in throttle] # RPM tracks throttle exactly + rows = [] + for i in range(n): + rows.append({"t": i * 0.01, "segment": "rampdn", + "throttle_cmd": throttle[i], "stand_rpm": rpm[i]}) + d = metricsmod.detect_demag( + RunResult(rows=rows), + Profile(name="synthetic", sample_rate_hz=100.0, + segments=[Segment(label="rampdn", throttle=0.0, + duration_s=4.0, ramp=True)])) + assert d["rpm_drop_samples"] == 0 + assert d["event_count"] == 0 + + +def test_collapse_during_ramp_up_still_detected(): + # The trend-based gate must not blind the detector to a real collapse + # that happens to occur while throttle is rising. + n = 60 + throttle = [0.5 + 0.4 * i / (n - 1) for i in range(n)] # 0.5 -> 0.9 rising + rpm = [20000.0 + 100.0 * i for i in range(n)] + for i in range(20, 30): + rpm[i] = 11000.0 # desync collapse mid-ramp, throttle still rising + rows = [] + for i in range(n): + rows.append({"t": i * 0.01, "segment": "rampup", + "throttle_cmd": throttle[i], "stand_rpm": rpm[i]}) + d = metricsmod.detect_demag( + RunResult(rows=rows), + Profile(name="synthetic", sample_rate_hz=100.0, + segments=[Segment(label="rampup", throttle=0.9, + duration_s=0.6, ramp=True)])) + assert d["rpm_drop_samples"] >= 5 + assert d["event_count"] >= 1 + + +def test_rig_pole_pairs_from_meta_overrides_profile(): + # rig meta says 11 pole pairs; profile default is 7. eRPM 154000 at + # 14000 stand RPM matches 11pp exactly -> no mismatch when meta is used. + n = 50 + rows = _rows(n, stand_rpm=lambda i: 14000.0, esc_erpm=lambda i: 154000) + d = metricsmod.detect_demag( + RunResult(meta={"pole_pairs": 11}, rows=rows), _profile()) + assert d["esc_rpm_mismatch_samples"] == 0 + d7 = metricsmod.detect_demag(RunResult(meta={}, rows=rows), _profile()) + assert d7["esc_rpm_mismatch_samples"] == n # wrong pp -> pervasive mismatch + + +def test_cpu_load_idle_rate_ignores_stall_glitch(): + # 120k iters/s idle; one sample pair spans a 100 ms host stall (11x the + # iteration delta) - a max-based idle rate would scale every other + # sample to ~91% load; the median must shrug it off. + n = 100 + rows = [] + iters = 0 + for i in range(n): + di = 1200 if i != 50 else 1200 * 11 # the stall sample + iters += di + rows.append({"t": i * 0.01, "segment": "idle", "throttle_cmd": 0.0, + "perf_loop_iters": iters}) + m = metricsmod._cpu_load(rows) + load, idle_rate = m + assert abs(idle_rate - 120000.0) < 1000.0 + + +def test_cpu_load_prefers_perf_host_t(): + # Same counter stream, but perf_host_t records the TRUE read times + # including the stall - the rate stays flat and load stays ~0 everywhere. + n = 100 + rows = [] + iters = 0 + t_true = 0.0 + for i in range(n): + dt_true = 0.01 if i != 50 else 0.11 # actual wall clock incl. stall + t_true += dt_true + iters += int(120000 * dt_true) + rows.append({"t": i * 0.01, "segment": "idle", "throttle_cmd": 0.0, + "perf_loop_iters": iters, "perf_host_t": t_true}) + load, idle_rate = metricsmod._cpu_load(rows) + assert abs(idle_rate - 120000.0) < 1500.0 + import numpy as np + assert np.nanmax(load) < 5.0 + + +def test_zc_jitter_from_accumulator_deltas(): + # 100 commutations per tick at a 200-tick mean interval with 2 ticks of + # mean deviation -> 1.0% jitter, computed from first/last snapshot deltas. + n = 100 + rows = _rows( + n, + perf_zc_count=lambda i: 100 * i, + perf_zc_jitter_sum=lambda i: 2 * 100 * i, + perf_zc_interval_sum=lambda i: 200 * 100 * i, + perf_zc_jitter_max=lambda i: 9, + ) + m = metricsmod.compute(RunResult(rows=rows), _profile()) + pt = m["steady_points"][0] + assert pt["zc_jitter_pct"] == 1.0 + assert pt["zc_jitter_max_pct"] == 4.5 # 9 / 200 + assert m["summary"]["worst_zc_jitter_pct"] == 1.0 + assert m["summary"]["worst_zc_jitter_max_pct"] == 4.5 + + +def test_zc_jitter_survives_u32_wrap(): + # interval_sum wraps u32 mid-window (it grows ~2M ticks/s on hardware); + # modular differencing must keep the ratio exact. + n = 100 + start = (1 << 32) - 200 * 100 * 50 # wraps halfway through the run + rows = _rows( + n, + perf_zc_count=lambda i: 100 * i, + perf_zc_jitter_sum=lambda i: 2 * 100 * i, + perf_zc_interval_sum=lambda i: (start + 200 * 100 * i) % (1 << 32), + perf_zc_jitter_max=lambda i: 9, + ) + m = metricsmod.compute(RunResult(rows=rows), _profile()) + assert m["steady_points"][0]["zc_jitter_pct"] == 1.0 + + +def test_zc_jitter_none_for_v1_runs(): + # Runs captured on pre-v2 firmware have no zc columns: the metric must + # report None (unavailable), never 0 (which would read as "perfect"). + rows = _rows(50, perf_loop_iters=lambda i: 1200 * i) + m = metricsmod.compute(RunResult(rows=rows), _profile()) + pt = m["steady_points"][0] + assert pt["zc_jitter_pct"] is None + assert pt["zc_jitter_max_pct"] is None + assert m["summary"]["worst_zc_jitter_pct"] is None + + +def test_zc_jitter_none_when_no_commutations_accumulate(): + # Counters present but frozen (motor stopped / startup-gated): no window + # delta, so the metric is unavailable rather than 0/0 noise. + rows = _rows( + 50, + perf_zc_count=lambda i: 5000, + perf_zc_jitter_sum=lambda i: 777, + perf_zc_interval_sum=lambda i: 999999, + perf_zc_jitter_max=lambda i: 3, + ) + m = metricsmod.compute(RunResult(rows=rows), _profile()) + assert m["steady_points"][0]["zc_jitter_pct"] is None diff --git a/hwci/tests/test_perf.py b/hwci/tests/test_perf.py new file mode 100644 index 000000000..db18de56c --- /dev/null +++ b/hwci/tests/test_perf.py @@ -0,0 +1,138 @@ +"""Tests for the hwci_perf struct decoder.""" +import struct + +import pytest + +from hwci import perf + + +def test_layout_sizes(): + assert perf.SIZE_BY_VERSION[1] == 64 + assert perf.SIZE_BY_VERSION[2] == 80 + assert perf.SIZE == perf.SIZE_BY_VERSION[3] == 96 + + +def test_host_cmd_offset_matches_layout(): + # host_cmd sits at 60 in EVERY version (v2+ fields append after it), so + # the reset-command write lands regardless of the flashed vintage. + assert perf.HOST_CMD_OFFSET == 60 + + +def test_roundtrip_encode_decode(): + sample = { + "ctrl_exec_us_max": 18, + "ctrl_period_us_min": 49, + "ctrl_period_us_max": 53, + "main_loop_us_max": 12, + "loop_iters": 123456, + "zero_cross_count": 9001, + "voltage_cv": 1612, # 16.12 V + "current_ca": 2550, # 25.50 A + "e_rpm": 321, # 32100 eRPM + "temperature_c": 47, + "armed": 1, + "running": 1, + "commutation_interval": 110, + } + blob = perf.encode(sample) + assert len(blob) == perf.SIZE + decoded = perf.decode(blob) + assert decoded.ctrl_exec_us_max == 18 + assert decoded.ctrl_period_us_min == 49 + assert decoded.loop_iters == 123456 + assert decoded.voltage == pytest.approx(16.12) + assert decoded.current == pytest.approx(25.50) + assert decoded.e_rpm == 32100 + + +def test_roundtrip_zc_jitter_fields(): + blob = perf.encode({ + "zc_count": 700000, + "zc_jitter_sum": 123456789, + "zc_interval_sum": 4000000000, # near the u32 wrap + "zc_jitter_max": 41, + }) + r = perf.decode(blob).raw + assert r["zc_count"] == 700000 + assert r["zc_jitter_sum"] == 123456789 + assert r["zc_interval_sum"] == 4000000000 + assert r["zc_jitter_max"] == 41 + + +def test_roundtrip_bdshot_fields(): + blob = perf.encode({ + "dshot_rx_good": 120000, + "dshot_rx_bad": 3, + "dshot_tx_frames": 119900, + "dshot_last_com_us": 420, + "dshot_telem_mode": 1, + "dshot_edt_mode": 1, + }) + r = perf.decode(blob).raw + assert r["dshot_rx_good"] == 120000 + assert r["dshot_rx_bad"] == 3 + assert r["dshot_tx_frames"] == 119900 + assert r["dshot_last_com_us"] == 420 + assert r["dshot_telem_mode"] == 1 + assert r["dshot_edt_mode"] == 1 + + +def test_v1_roundtrip_has_no_zc_keys(): + # Old firmware (A side of an A/B session) reports a 64-byte v1 struct; + # it must decode cleanly and simply not carry the jitter fields. + blob = perf.encode({"ctrl_exec_us_max": 18, "loop_iters": 42}, version=1) + assert len(blob) == perf.SIZE_BY_VERSION[1] + s = perf.decode(blob) + assert s.ctrl_exec_us_max == 18 + assert s.loop_iters == 42 + assert "zc_count" not in s.raw + assert "dshot_rx_good" not in s.raw + + +def test_v2_roundtrip_has_no_bdshot_keys(): + blob = perf.encode({"loop_iters": 9, "zc_count": 11}, version=2) + assert len(blob) == perf.SIZE_BY_VERSION[2] + s = perf.decode(blob) + assert s.loop_iters == 9 + assert s.raw["zc_count"] == 11 + assert "dshot_rx_good" not in s.raw + + +def test_v1_decodes_from_oversized_read(): + # PerfReader sizes its read from the ELF symbol, but a v3-sized buffer + # holding a v1 struct (plus trailing junk) must still decode as v1. + blob = perf.encode({"loop_iters": 7}, version=1) + b"\xa5" * 32 + s = perf.decode(blob) + assert s.loop_iters == 7 + assert "zc_count" not in s.raw + + +def test_mech_rpm(): + blob = perf.encode({"e_rpm": 140}) # 14000 eRPM + s = perf.decode(blob) + assert s.mech_rpm(7) == pytest.approx(2000.0) + + +def test_bad_magic_raises(): + blob = bytearray(perf.encode({})) + struct.pack_into(" None: + super().write_u32(addr, 0) # command applied and cleared + + +def test_reset_stats_verify_passes_when_consumed(host_perf_elf): + from hwci import elf as elfmod + addr = elfmod.find_symbol(host_perf_elf, "hwci_perf").address + dbg = _FirmwareLikeMock(base=addr, size=perf.SIZE + 256) + reader = PerfReader(dbg, host_perf_elf) + dbg.poke(reader.address, perf.encode({})) + reader.reset_stats() # must not raise + + +def test_reset_stats_verify_times_out_when_not_consumed(host_perf_elf): + from hwci.debugger.base import DebuggerError + reader, dbg = _reader_with_mock(host_perf_elf) + dbg.poke(reader.address, perf.encode({})) + with pytest.raises(DebuggerError): + reader.reset_stats(timeout_s=0.05) + + +def test_layout_check_passes_for_real_header(host_perf_elf): + # Should not raise: DWARF layout matches canonical perf.FIELDS. + reader, _ = _reader_with_mock(host_perf_elf) + assert reader.address > 0 + + +def test_v1_firmware_reads_and_resets(host_perf_elf_v1): + # The A side of an A/B session runs old firmware with the 64-byte v1 + # struct: the reader must size its SWD read from the ELF, decode without + # the zc_* fields, pass the DWARF layout cross-check against the v1 + # canonical table, and land RESET_STATS at the version-stable offset 60. + reader, dbg = _reader_with_mock(host_perf_elf_v1) + assert reader._read_size == perf.SIZE_BY_VERSION[1] + dbg.poke(reader.address, perf.encode( + {"ctrl_exec_us_max": 33, "loop_iters": 999}, version=1)) + sample = reader.read() + assert sample.ctrl_exec_us_max == 33 + assert sample.loop_iters == 999 + assert "zc_count" not in sample.raw + reader.reset_stats(verify=False) + word = dbg.read_memory(reader.address + perf.HOST_CMD_OFFSET, 4) + assert int.from_bytes(word, "little") == perf.CMD_RESET_STATS + + +def test_missing_struct_die_fails_hard(host_perf_elf, monkeypatch): + # DWARF present but the struct tag gone (rename/LTO): decoding on faith is + # exactly the drift the cross-check exists to catch -> must raise. + import hwci.perf_reader as pr + monkeypatch.setattr(pr, "STRUCT_TAG", "definitely_not_a_struct") + from hwci import elf as elfmod + addr = elfmod.find_symbol(host_perf_elf, "hwci_perf").address + dbg = MockDebugger(base=addr, size=perf.SIZE + 256) + with pytest.raises(perf.PerfDecodeError): + PerfReader(dbg, host_perf_elf) diff --git a/hwci/tests/test_runner_safety.py b/hwci/tests/test_runner_safety.py new file mode 100644 index 000000000..2754dab16 --- /dev/null +++ b/hwci/tests/test_runner_safety.py @@ -0,0 +1,78 @@ +"""Runner-level (host-side) safety enforcement - must trip on ANY rig, even +when the stand backend does no checking of its own.""" +from hwci.config import Profile, Segment +from hwci.esc_telem.kiss import KissFrame +from hwci.flightstand.base import SafetyLimits +from hwci.flightstand.simulator import SimulatedStand +from hwci.runner import Sources, run_profile +from hwci.sim import MotorParams, RigSimulator +from hwci.throttle.flightstand_src import FlightStandThrottle + + +def _profile(**safety): + return Profile( + name="safety-test", + sample_rate_hz=100.0, + segments=[Segment(label="wot", throttle=1.0, duration_s=2.0)], + safety=SafetyLimits(**safety), + ) + + +def _frame(current_a: float, temp_c: int = 30) -> KissFrame: + return KissFrame(temperature_c=temp_c, voltage_v=16.0, current_a=current_a, + consumption_mah=0, e_rpm=10000, crc_ok=True) + + +def test_runner_trips_on_stand_current_without_backend_limits(): + # The stand itself gets NO limits (mirrors the gRPC backend, which cannot + # enforce them until the vendor RPC is mapped) - the runner must trip. + sim = RigSimulator(params=MotorParams(), noise=0.0) + stand = SimulatedStand(sim, fixed_dt=0.01).open() + sources = Sources( + throttle=FlightStandThrottle(stand, arm_settle_s=0.0), + stand=stand, + perf_source=lambda: None, + telem_source=lambda: None, + ) + result = run_profile(_profile(max_current_a=5.0), sources, realtime=False) + assert result.meta["aborted"] is not None + assert "safety" in result.meta["aborted"] + assert "current" in result.meta["aborted"] + + +def test_runner_trips_on_telemetry_only_rig(): + # No stand at all: the ESC telemetry current must still enforce the limit. + class _DummyThrottle: + def arm(self): pass + def set(self, throttle): pass + def disarm(self): pass + def close(self): pass + + sources = Sources( + throttle=_DummyThrottle(), + stand=None, + perf_source=lambda: None, + telem_source=lambda: _frame(current_a=60.0), + ) + result = run_profile(_profile(max_current_a=45.0), sources, realtime=False) + assert result.meta["aborted"] is not None + assert "current" in result.meta["aborted"] + + +def test_standless_run_completes_within_limits(): + class _DummyThrottle: + def arm(self): pass + def set(self, throttle): pass + def disarm(self): pass + def close(self): pass + + sources = Sources( + throttle=_DummyThrottle(), + stand=None, + perf_source=lambda: None, + telem_source=lambda: _frame(current_a=10.0), + ) + result = run_profile(_profile(max_current_a=45.0), sources, realtime=False) + assert result.meta["aborted"] is None + assert len(result.rows) == 200 + assert result.rows[0]["stand_thrust_gf"] == "" # stand columns empty diff --git a/hwci/tests/test_runner_sim.py b/hwci/tests/test_runner_sim.py new file mode 100644 index 000000000..813e646dc --- /dev/null +++ b/hwci/tests/test_runner_sim.py @@ -0,0 +1,56 @@ +"""End-to-end simulated runs through the runner + metrics + persistence.""" +from hwci import metrics as metricsmod +from hwci.config import RigConfig, load_profile +from hwci.model import RunResult +from hwci.runner import build_sim_sources, run_profile + + +def _run(profile_name, demag_prone=True): + rig = RigConfig() + profile = load_profile(profile_name) + sources = build_sim_sources(rig, profile, demag_prone=demag_prone) + try: + return run_profile(profile, sources, realtime=False, + meta={"target": "ARK_4IN1_F051"}), profile + finally: + sources.close() + + +def test_ci_smoke_runs_and_has_steady_points(): + result, profile = _run("ci_smoke") + assert result.meta["aborted"] is None + assert len(result.rows) > 100 + m = metricsmod.compute(result, profile) + assert len(m["steady_points"]) == 2 # hold25, hold45 + assert m["summary"]["max_thrust_gf"] > 0 + assert m["summary"]["peak_efficiency_gf_per_w"] > 0 + assert 0 <= m["summary"]["max_cpu_load_pct"] <= 100 + # Smooth ramps must not be flagged as demag. + assert m["summary"]["demag_events"] == 0 + + +def test_efficiency_sweep_curve_is_monotonic_thrust(): + result, profile = _run("efficiency_sweep") + m = metricsmod.compute(result, profile) + thrusts = [p["thrust_gf"] for p in m["steady_points"]] + assert thrusts == sorted(thrusts) # thrust rises with throttle + assert len(m["steady_points"]) == 10 + + +def test_demag_profile_detects_events(): + result, profile = _run("demag_step_stress", demag_prone=True) + m = metricsmod.compute(result, profile) + assert m["summary"]["demag_events"] >= 1 + assert m["demag"]["bemf_timeout_samples"] >= 1 + + +def test_save_load_roundtrip(tmp_path): + result, profile = _run("ci_smoke") + result.save(tmp_path / "run") + loaded = RunResult.load(tmp_path / "run") + assert len(loaded.rows) == len(result.rows) + assert loaded.meta["profile"] == "ci_smoke" + # metrics identical after roundtrip + m1 = metricsmod.compute(result, profile) + m2 = metricsmod.compute(loaded, profile) + assert m1["summary"]["max_thrust_gf"] == m2["summary"]["max_thrust_gf"] diff --git a/hwci/tests/test_sim.py b/hwci/tests/test_sim.py new file mode 100644 index 000000000..8502cdd2d --- /dev/null +++ b/hwci/tests/test_sim.py @@ -0,0 +1,82 @@ +"""Tests for the rig simulator: cross-channel consistency + demag injection.""" +import pytest + +from hwci import perf +from hwci.esc_telem import parse_frame +from hwci.sim import MotorParams, RigSimulator + + +def _settle(rig, throttle, n=200, dt=0.005): + for _ in range(n): + rig.step(dt, throttle) + + +def test_thrust_increases_with_throttle(): + lo = RigSimulator(noise=0.0) + hi = RigSimulator(noise=0.0) + _settle(lo, 0.3) + _settle(hi, 0.8) + assert hi.thrust_n > lo.thrust_n > 0 + + +def test_efficiency_is_plausible(): + rig = RigSimulator(noise=0.0) + _settle(rig, 0.5) + s = rig.stand_sample(1.0) + # A few g/W is realistic for a loaded 5" setup. + assert 1.0 < s.efficiency_gf_per_w < 15.0 + + +def test_kiss_channel_matches_state(): + rig = RigSimulator(noise=0.0) + _settle(rig, 0.6) + frame = parse_frame(rig.kiss_bytes()) + assert frame.crc_ok + assert frame.voltage_v == pytest.approx(rig.voltage, abs=0.05) + assert frame.e_rpm == pytest.approx(rig.e_rpm, rel=0.02, abs=200) + + +def test_perf_channel_decodes_and_is_consistent(): + rig = RigSimulator(noise=0.0) + _settle(rig, 0.7) + sample = perf.decode(rig.perf_bytes()) + assert sample.raw["running"] == 1 + assert 0 < sample.raw["commutation_interval_max"] + assert sample.loop_iters > 0 + assert sample.e_rpm == pytest.approx(rig.e_rpm, rel=0.02, abs=200) + + +def test_demag_injection_triggers_desync(): + rig = RigSimulator(params=MotorParams(demag_prone=True), noise=0.0) + _settle(rig, 0.1, n=50) + rpm_before = rig.rpm + rig.step(0.005, 1.0) # violent step into a high-current regime + assert rig.desync_count >= 1 + # Sync loss collapses RPM rather than spooling up. + assert rig.rpm < rpm_before * 1.1 + sample = perf.decode(rig.perf_bytes()) + assert sample.raw["bemf_timeout_state"] == 1 + + +def test_no_demag_when_not_prone(): + rig = RigSimulator(params=MotorParams(demag_prone=False), noise=0.0) + _settle(rig, 0.1, n=50) + rig.step(0.005, 1.0) + assert rig.desync_count == 0 + + +def test_perf_channel_carries_zc_jitter_accumulators(): + rig = RigSimulator(noise=0.0) + _settle(rig, 0.7) + a = perf.decode(rig.perf_bytes()).raw + _settle(rig, 0.7) + b = perf.decode(rig.perf_bytes()).raw + # monotonic sums, growing while running + assert b["zc_count"] > a["zc_count"] > 0 + assert b["zc_jitter_sum"] > a["zc_jitter_sum"] + assert b["zc_interval_sum"] > a["zc_interval_sum"] + # mean fractional jitter is the sim's modeled ~0.5% of the interval + mean_pct = 100.0 * (b["zc_jitter_sum"] - a["zc_jitter_sum"]) / ( + b["zc_interval_sum"] - a["zc_interval_sum"]) + assert 0.1 < mean_pct < 2.0 + assert b["zc_jitter_max"] >= 1 diff --git a/hwci/tests/test_tail_reset_margin.py b/hwci/tests/test_tail_reset_margin.py new file mode 100644 index 000000000..b5d597d2d --- /dev/null +++ b/hwci/tests/test_tail_reset_margin.py @@ -0,0 +1,33 @@ +"""_tail_reset_tick: the mid-segment perf-stats reset must land strictly +before the metrics tail window, not at its boundary. + +reset_stats() clears the firmware register over an SWD round trip, and in +realtime mode perf_get() reads an independent background poller's CACHED +value (~2ms interval) - so the tick that ISSUES the reset can still observe +the pre-reset cached sample. Observed on the bench: an 877us arming-tune +transient latched at a segment's first tick was still visible in the exact +sample the reset was issued on, making the "steady" max equal the raw run +max. The fix needs a margin, not an exact boundary match - and that margin +must be measured against the SAME tail_start_index() metrics.py uses: an +earlier version of this fix recomputed the tail boundary with its own +independently-rounded formula (round() vs metrics.py's int() truncation), +which silently disagreed by a tick for some segment lengths (e.g. n=300, +fraction=0.8) - exactly the kind of gap this race hides in. +""" +from hwci.metrics import tail_start_index +from hwci.runner import RESET_MARGIN_TICKS, _tail_reset_tick + + +def test_reset_lands_before_metrics_tail_with_margin(): + for n, fraction in [(1000, 0.5), (600, 0.5), (100, 0.3), (300, 0.8)]: + reset_tick = _tail_reset_tick(n, fraction) + tail_start = tail_start_index(n, fraction) + assert reset_tick <= tail_start - RESET_MARGIN_TICKS, ( + f"n={n} fraction={fraction}: reset_tick={reset_tick} leaves " + f"less than {RESET_MARGIN_TICKS}-tick margin before tail_start={tail_start}") + + +def test_reset_tick_never_negative_on_short_segments(): + # A segment shorter than the margin must clamp to 0, not go negative. + assert _tail_reset_tick(5, 0.5) == 0 + assert _tail_reset_tick(1, 0.9) == 0 diff --git a/hwci/tests/test_tare.py b/hwci/tests/test_tare.py new file mode 100644 index 000000000..f654ba97f --- /dev/null +++ b/hwci/tests/test_tare.py @@ -0,0 +1,55 @@ +"""Pre-run tare: zero the load cells only AFTER the ESC signal is up at zero +throttle. AM32 beeps the motor whenever it has NO input signal (and again as +it arms), and each beep is a torque pulse through the mount - a tare taken on +a beeping ESC bakes that twitching into the load-cell zero (seen on the bench +as a wandering thrust offset between runs).""" +from types import SimpleNamespace + +from hwci.runner import ARM_TUNE_SETTLE_S, TARE_SETTLE_S, tare_for_run + + +class _Rig: + """Fake stand + throttle sharing ONE event log, so tests can assert + ordering ACROSS the two objects - the whole point of the choreography.""" + + def __init__(self, thrust_n=0.0, read_raises=False): + self.events: list = [] + self._thrust_n = thrust_n + self._read_raises = read_raises + self.throttle = SimpleNamespace(arm=lambda: self.events.append("arm")) + self.stand = SimpleNamespace(tare=lambda: self.events.append("tare"), + read_sample=self._read_sample) + + def _read_sample(self): + self.events.append("read") + if self._read_raises: + raise RuntimeError("stand went away") + return SimpleNamespace(thrust_n=self._thrust_n) + + def settle(self, seconds: float) -> None: + self.events.append(("settle", seconds)) + + +def test_signal_is_up_and_arm_tune_finished_before_tare(): + rig = _Rig() + tare_for_run(rig.stand, rig.throttle, settle=rig.settle) + assert rig.events == [ + "arm", # signal at zero: beacon stops + ("settle", ARM_TUNE_SETTLE_S), # arm tune stops shaking the motor + "tare", + ("settle", TARE_SETTLE_S), # post-tare readings settle + "read", + ] + + +def test_residual_is_reported_in_gf(): + rig = _Rig(thrust_n=0.0980665) # exactly 10 gf + residual = tare_for_run(rig.stand, rig.throttle, settle=rig.settle) + assert abs(residual - 10.0) < 1e-9 + + +def test_residual_read_is_best_effort_but_tare_is_not(): + rig = _Rig(read_raises=True) + residual = tare_for_run(rig.stand, rig.throttle, settle=rig.settle) + assert residual is None # unreadable residual doesn't kill the run + assert "tare" in rig.events # ... but the tare itself already happened diff --git a/hwci/tests/test_throttle_map.py b/hwci/tests/test_throttle_map.py new file mode 100644 index 000000000..691ea48d6 --- /dev/null +++ b/hwci/tests/test_throttle_map.py @@ -0,0 +1,58 @@ +"""DShot throttle mapping: zero throttle must be DShot 0, never 1-47. + +AM32 only arms on sustained DShot 0 (verified on the ARK 4IN1 bench: at a +constant DShot 48 the ESC decodes input=48 but never arms, Src/main.c requires +adjusted_input == 0 for one second). Values 1-47 are DShot COMMANDS - a ramp +must never sweep through them. +""" +from __future__ import annotations + +import pytest + +from hwci.flightstand.grpc_client import FlightStandGrpc, SignalMap + + +class RecordingApi: + def __init__(self): + self.calls = [] + + def set_output(self, output_id, value, *, active=True): + self.calls.append((output_id, value, active)) + + +def make_stand(**signal_overrides) -> tuple[FlightStandGrpc, RecordingApi]: + stand = FlightStandGrpc(signals=SignalMap(**signal_overrides)) + stand._api = RecordingApi() + return stand, stand._api + + +def test_dshot_zero_throttle_is_dshot_zero(): + stand, api = make_stand(esc_zero=0.0, esc_min=48.0, esc_max=2047.0) + stand.set_throttle(0.0) + assert api.calls[-1] == (0, 0.0, True) + + +def test_dshot_positive_throttle_starts_at_48_never_commands(): + stand, api = make_stand(esc_zero=0.0, esc_min=48.0, esc_max=2047.0) + for t in (1e-6, 0.001, 0.01, 0.10, 1.0): + stand.set_throttle(t) + raw = api.calls[-1][1] + assert raw >= 48.0, f"throttle {t} emitted DShot command value {raw}" + stand.set_throttle(1.0) + assert api.calls[-1][1] == pytest.approx(2047.0) + + +def test_pwm_default_zero_is_esc_min(): + # esc_zero=None: standard PWM where 1000 us is both zero and idle + stand, api = make_stand(esc_min=1000.0, esc_max=2000.0) + stand.set_throttle(0.0) + assert api.calls[-1][1] == pytest.approx(1000.0) + stand.set_throttle(0.5) + assert api.calls[-1][1] == pytest.approx(1500.0) + + +def test_close_parks_at_esc_zero_then_deactivates(): + stand, api = make_stand(esc_zero=0.0, esc_min=48.0, esc_max=2047.0) + stand.close() + assert api.calls[-2] == (0, 0.0, True) + assert api.calls[-1] == (0, 0.0, False) diff --git a/make/tools.mk b/make/tools.mk index b4de393c6..f3dc6ca46 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -6,6 +6,16 @@ ifeq ($(OS),Windows_NT) +UNAME_O := $(shell uname -o 2>/dev/null) +# only Cygwin gets the unix tools flow. cmd.exe and git-bash/MSYS keep +# the cmd.exe flow: git-bash may put uname on PATH but its MinGW tools +# cannot build the SITL, and the cmd.exe flow is proven there +ifneq ($(UNAME_O),Cygwin) +WIN_CMD_FLOW := 1 +endif +endif + +ifeq ($(WIN_CMD_FLOW),1) ARM_SDK_PREFIX:=tools/windows/xpack-arm-none-eabi-gcc-10.3.1-2.3/bin/arm-none-eabi- SHELL:=cmd.exe CP:=tools\\windows\\make\\bin\\cp @@ -17,6 +27,18 @@ CUT:=tools\\windows\\make\\bin\\cut FGREP:=tools\\windows\\make\\bin\\fgrep OSDIR:=windows +else ifeq ($(OS),Windows_NT) +# Cygwin +ARM_SDK_PREFIX:=tools/windows/xpack-arm-none-eabi-gcc-10.3.1-2.3/bin/arm-none-eabi- +CP:=cp +DSEP:=/ +NUL:=/dev/null +MKDIR:=mkdir +RM:=rm +CUT:=cut +FGREP:=fgrep +OSDIR:=windows + else # MacOS and Linux UNAME_S := $(shell uname -s) diff --git a/sitl_gui.bat b/sitl_gui.bat new file mode 100644 index 000000000..6fcf8d6e8 --- /dev/null +++ b/sitl_gui.bat @@ -0,0 +1,11 @@ +@echo off +rem launch the AM32 SITL GUI using the environment created by +rem "py Mcu\SITL\make_gui_env.py". Lives in the repo root so the venv +rem paths resolve relative to it +cd /d "%~dp0" +if not exist "Mcu\SITL\venv\Scripts\pythonw.exe" ( + echo GUI environment not found, run: py Mcu\SITL\make_gui_env.py + pause + exit /b 1 +) +start "" "Mcu\SITL\venv\Scripts\pythonw.exe" "Mcu\SITL\sitl_gui.py" %* diff --git a/sitlmakefile.mk b/sitlmakefile.mk new file mode 100644 index 000000000..115641328 --- /dev/null +++ b/sitlmakefile.mk @@ -0,0 +1,72 @@ +MCU := SITL + +MCU_LC := $(call lc,$(MCU)) + +ifeq ($(OS),Windows_NT) +ifeq ($(UNAME_O),Cygwin) +TARGETS_$(MCU) := $(call get_targets,$(MCU)) +else +# plain Windows (cmd.exe or git-bash/MinGW): no POSIX environment, the +# SITL only builds under Cygwin there +TARGETS_$(MCU) := +endif +else +TARGETS_$(MCU) := $(call get_targets,$(MCU)) +endif + +HAL_FOLDER_$(MCU) := $(HAL_FOLDER)/$(MCU) + +# native build using the host compiler +SITL_CC := gcc +SITL_OBJCOPY := objcopy +NATIVE_$(MCU) := 1 + +MCU_$(MCU) := +LDSCRIPT_$(MCU) := + +SRC_DIR_$(MCU) := \ + $(HAL_FOLDER_$(MCU))/Src \ + $(HAL_FOLDER_$(MCU))/sim + +CFLAGS_$(MCU) := \ + -I$(HAL_FOLDER_$(MCU))/Inc \ + -I$(HAL_FOLDER_$(MCU))/sim + +CFLAGS_$(MCU) += -D_GNU_SOURCE +# newlib based hosts (Cygwin) have no __WORDSIZE for the canard.h default +CFLAGS_$(MCU) += -DCANARD_64_BIT="(__SIZEOF_POINTER__ == 8)" + +# native compiler flags, replacing the ARM specific CFLAGS_COMMON. Inc is +# searched via -iquote rather than -I so that Inc/signal.h does not shadow +# the system +ifeq ($(UNAME_S),Darwin) +# clang: no -fsingle-precision-constant, and ignore the gcc-only +# -Wno- options inherited from the common CFLAGS +SITL_GCC_FLAGS := -Wno-unknown-warning-option +else +SITL_GCC_FLAGS := -fsingle-precision-constant -Wno-stringop-truncation +endif +# -funsigned-char matches the ARM targets, where char is unsigned +CFLAGS_COMMON_$(MCU) := $(SITL_GCC_FLAGS) -funsigned-char -iquote $(MAIN_INC_DIR) -g3 -O2 \ + -Wall -Wundef -Wextra -Werror -Wno-unused-parameter \ + -fno-strict-aliasing -pthread + +LDFLAGS_COMMON_$(MCU) := -pthread + +LDLIBS_$(MCU) := -lm + +SRC_$(MCU) := $(foreach dir,$(SRC_DIR_$(MCU)),$(wildcard $(dir)/*.c)) + +# optional CAN support +CFLAGS_CAN_$(MCU) = \ + -ISrc/DroneCAN \ + -ISrc/DroneCAN/libcanard \ + -ISrc/DroneCAN/dsdl_generated/include + +SRC_DIR_CAN_$(MCU) = Src/DroneCAN \ + Src/DroneCAN/dsdl_generated/src \ + Src/DroneCAN/libcanard + +SRC_CAN_$(MCU) := $(foreach dir,$(SRC_DIR_CAN_$(MCU)),$(wildcard $(dir)/*.[cs])) + +LDSCRIPT_CAN_$(MCU) :=