diff --git a/CMakeLists.txt b/CMakeLists.txt index 46b180db9..681e0667a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,10 @@ project(HDZGOGGLE VERSION 1.1) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(COMMON_COMPILER_FLAGS "-Wno-unused-function -Wno-unused-variable -Wno-deprecated-declarations -ffunction-sections -fdata-sections -Wl,-gc-sections -D_LARGEFILE_SOURCE -D_LARGEFILE64_SOURCE -D_FILE_OFFSET_BITS=64") +set(COMMON_COMPILER_FLAGS "-Wno-unused-function -Wno-unused-variable -Wno-deprecated-declarations -ffunction-sections -fdata-sections -D_LARGEFILE_SOURCE -D_LARGEFILE64_SOURCE -D_FILE_OFFSET_BITS=64") +if(NOT APPLE) + set(COMMON_COMPILER_FLAGS "${COMMON_COMPILER_FLAGS} -Wl,-gc-sections") +endif() option(EMULATOR_BUILD "emulate using SDL on host" OFF) if(NOT EMULATOR_BUILD) diff --git a/docs/synthetic-motor-audio.md b/docs/synthetic-motor-audio.md new file mode 100644 index 000000000..355cfcc0b --- /dev/null +++ b/docs/synthetic-motor-audio.md @@ -0,0 +1,59 @@ +# Synthetic Motor Audio (Goggle side) + +Proof-of-concept: synthesize stereo propeller sound from mechanical RPM carried in the HDZero `0xFF` service packet. Pair with the matching VTX change (same packing). No UI, no hex/octo, no new RF opcodes. + +## Packet (VRX view) + +`rx_buf[0]` is the payload length. Legacy packets use `15`. Extended packets use `21`: + +| Offset | Content | +|--------|---------| +| 0 | length (`21`) | +| 1..15 | legacy service fields (unchanged) | +| 16..21 | six packed 12-bit RPM bytes | + +Unpack / dequantize (`q * 32`) lives in `src/core/motor_rpm_proto.*`. Decode runs in `parser_config()` when `length >= 21`; shorter packets leave audio silent. + +Packing (must match VTX): + +``` +q = min((rpm + 16) >> 5, 4095) # 32 RPM steps, max 131040 +b0 = q0[7:0] +b1 = q1[3:0] << 4 | q0[11:8] +b2 = q1[11:4] +b3 = q2[7:0] +b4 = q3[3:0] << 4 | q2[11:8] +b5 = q3[11:4] +``` + +## Audio engine + +`src/core/motor_audio.*` — platform-independent stereo PCM at **22050 Hz** (enough for blade-pass + harmonics; half the CPU of 48 kHz). + +- QUADX pan: fronts louder, left/right stereo +- Blade-pass pulse, shaft asymmetry, quiet air noise, soft LP +- Stale telemetry (>500 ms) or all-zero RPM fades to silence + +## Menu / settings + +**Record Option → Motor Audio** (`On` / `Off`), stored as `record.motor_audio` in `setting.ini` (default `Off`). + +- Independent of **Record Audio** — this is a live headphone mix, not a DVR flag. +- Keeps the selected **Audio Source** (Mic / Line In / A/V In); when On, line-out also opens the DAC path (`audio_sel.sh out_dac_on`) so synthetic PCM can sit under the analog source. +- Emulator: toggles the SDL playback device. Hardware still needs a Softwinner AO PCM feed into that DAC path (not wired yet); RPM decode and mute gate are ready. + +## Host / emulator tooling + +Prefer the small host tool (full emulator needs Linux/ARM deps): + +```bash +cmake -S utilities/motor_audio_poc -B tmp/motor_audio_poc_build +cmake --build tmp/motor_audio_poc_build +./tmp/motor_audio_poc_build/motor_audio_poc --motor-test +./tmp/motor_audio_poc_build/motor_audio_poc --motor-synthetic --motor-wav tmp/motor_synthetic.wav +./tmp/motor_audio_poc_build/motor_audio_poc --motor-replay path.csv --motor-poles 12 --motor-wav tmp/out.wav +``` + +Blackbox CSV needs `time` / `time (us)` and `eRPM[0..3]` or `eRPM(/100)[0..3]`. Mechanical RPM = `eRPM * 2 / poles` (with `×100` when the column is `/100`). + +Emulator binary also accepts the same `--motor-*` flags when built with `EMULATOR_BUILD`. diff --git a/src/core/dvr.c b/src/core/dvr.c index 2956fe7d1..0b304b302 100644 --- a/src/core/dvr.c +++ b/src/core/dvr.c @@ -54,7 +54,11 @@ void dvr_enable_line_out(bool enable) { system_exec(buf); snprintf(buf, sizeof(buf), "%s out_linein_on", AUDIO_SEL_SH); system_exec(buf); - snprintf(buf, sizeof(buf), "%s out_dac_off", AUDIO_SEL_SH); + /* Keep selected analog source; open DAC only when motor audio mix is on. */ + if (g_setting.record.motor_audio) + snprintf(buf, sizeof(buf), "%s out_dac_on", AUDIO_SEL_SH); + else + snprintf(buf, sizeof(buf), "%s out_dac_off", AUDIO_SEL_SH); system_exec(buf); } else { snprintf(buf, sizeof(buf), "%s out_off", AUDIO_SEL_SH); diff --git a/src/core/main.c b/src/core/main.c index 01b4ae304..0fe6bf8df 100644 --- a/src/core/main.c +++ b/src/core/main.c @@ -15,6 +15,8 @@ #ifdef EMULATOR_BUILD #include "SDLaccess.h" +#include "emulator/motor_audio_dev.h" +#include "emulator/motor_audio_sdl.h" SDL_mutex *global_sdl_mutex; #endif @@ -24,6 +26,7 @@ SDL_mutex *global_sdl_mutex; #include "core/elrs.h" #include "core/ht.h" #include "core/input_device.h" +#include "core/motor_audio.h" #include "core/osd.h" #include "core/self_test.h" #include "core/settings.h" @@ -147,6 +150,11 @@ void lvgl_init() { } int main(int argc, char *argv[]) { +#ifdef EMULATOR_BUILD + if (motor_audio_dev_main(argc, argv)) + return 0; +#endif + pthread_mutex_init(&lvgl_mutex, NULL); #ifdef EMULATOR_BUILD @@ -184,6 +192,11 @@ int main(int argc, char *argv[]) { elrs_init(); ht_init(); beep_init(); + motor_audio_set_enabled(g_setting.record.motor_audio ? 1 : 0); +#ifdef EMULATOR_BUILD + if (g_setting.record.motor_audio) + motor_audio_sdl_start(MOTOR_AUDIO_DEFAULT_RATE); +#endif // 4. Initilize UI lvgl_init(); diff --git a/src/core/motor_audio.c b/src/core/motor_audio.c new file mode 100644 index 000000000..cd0611985 --- /dev/null +++ b/src/core/motor_audio.c @@ -0,0 +1,351 @@ +#include "motor_audio.h" + +#include + +#ifdef MOTOR_AUDIO_POC_HOST +float sinf(float); +float expf(float); +float fabsf(float); +long lrintf(float); +#else +#include +#endif + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +/* + * More realistic (still lightweight) FPV quad sound: + * - blade-pass loading pulses (not pure sines) + * - shaft-rate blade asymmetry + * - BPF-modulated broadband turbulence / "air" + * - weak motor electrical buzz + * - per-motor detune so the four props don't phase-lock into a tone + */ + +typedef struct { + float phase; /* blade-pass phase, radians */ + float shaft_phase; /* one revolution phase */ + float rpm_smooth; + float rpm_target; + float detune; /* static fractional detune */ + float buzz_phase; + float noise_lp; /* 1-pole LP state for this motor's air noise */ + float noise_hp; /* DC block / mild HP */ + uint32_t rng; +} motor_osc_t; + +static motor_osc_t g_motors[MOTOR_AUDIO_MOTORS]; +static uint32_t g_last_rpm_ms; +static uint8_t g_have_rpm; +static uint8_t g_enabled = 1; /* default on for host tests; UI/settings override */ +static float g_master_gain; +static int16_t g_peak_abs; +static uint32_t g_render_ms; +static uint32_t (*g_time_fn)(void); +static float g_mix_lp_l, g_mix_lp_l2; +static float g_mix_lp_r, g_mix_lp_r2; + +/* QUADX: motor index -> left/right gain and front/rear level. */ +static const float g_pan_l[MOTOR_AUDIO_MOTORS] = { + 0.22f, /* 1 RR */ + 0.28f, /* 2 FR */ + 0.95f, /* 3 RL */ + 1.00f, /* 4 FL */ +}; +static const float g_pan_r[MOTOR_AUDIO_MOTORS] = { + 0.95f, /* 1 RR */ + 1.00f, /* 2 FR */ + 0.22f, /* 3 RL */ + 0.28f, /* 4 FL */ +}; +static const float g_front_rear[MOTOR_AUDIO_MOTORS] = { + 0.68f, /* 1 RR rear quieter */ + 1.00f, /* 2 FR */ + 0.68f, /* 3 RL rear quieter */ + 1.00f, /* 4 FL */ +}; + +static float clampf(float x, float lo, float hi) { + if (x < lo) + return lo; + if (x > hi) + return hi; + return x; +} + +static float softsat(float x) { + /* Gentle tanh-ish soft clip without libm tanh */ + float a = fabsf(x); + float y = a / (1.0f + 0.55f * a); + return (x < 0.0f) ? -y : y; +} + +static uint32_t xorshift32(uint32_t *s) { + uint32_t x = *s; + x ^= x << 13; + x ^= x >> 17; + x ^= x << 5; + *s = x ? x : 0xA5A5A5A5u; + return *s; +} + +static float frand(uint32_t *s) { + /* approx uniform [-1, 1] */ + return ((float)(xorshift32(s) & 0xFFFFFF) * (2.0f / 16777215.0f)) - 1.0f; +} + +void motor_audio_set_enabled(int enabled) { + g_enabled = enabled ? 1 : 0; + if (!g_enabled) + g_master_gain = 0.0f; +} + +int motor_audio_is_enabled(void) { + return g_enabled ? 1 : 0; +} + +void motor_audio_set_time_fn(uint32_t (*fn)(void)) { + g_time_fn = fn; +} + +uint32_t motor_audio_now_ms(void) { + if (g_time_fn) + return g_time_fn(); + { + extern uint32_t time_ms(void); + return time_ms(); + } +} + +uint32_t motor_audio_render_clock_ms(void) { + return g_render_ms; +} + +void motor_audio_reset(void) { + uint32_t m; + memset(g_motors, 0, sizeof(g_motors)); + /* Spread initial phases / detune so motors don't start as one oscillator. */ + for (m = 0; m < MOTOR_AUDIO_MOTORS; m++) { + g_motors[m].phase = (float)m * 0.9f; + g_motors[m].shaft_phase = (float)m * 1.3f; + g_motors[m].buzz_phase = (float)m * 2.1f; + /* +/- ~0.6% detune — enough to break beating into a chord */ + g_motors[m].detune = 1.0f + (0.0035f * (float)((int)m - 1) - 0.0015f); + g_motors[m].rng = 0x12345678u ^ (m * 0x9E3779B9u); + } + g_last_rpm_ms = 0; + g_have_rpm = 0; + g_master_gain = 0.0f; + g_peak_abs = 0; + g_render_ms = 0; + g_mix_lp_l = g_mix_lp_l2 = 0.0f; + g_mix_lp_r = g_mix_lp_r2 = 0.0f; +} + +void motor_audio_set_rpm(uint32_t rpm0, uint32_t rpm1, uint32_t rpm2, uint32_t rpm3, uint32_t timestamp_ms) { + g_motors[0].rpm_target = (float)rpm0; + g_motors[1].rpm_target = (float)rpm1; + g_motors[2].rpm_target = (float)rpm2; + g_motors[3].rpm_target = (float)rpm3; + g_last_rpm_ms = timestamp_ms; + g_have_rpm = 1; +} + +int16_t motor_audio_peak_abs(void) { + return g_peak_abs; +} + +static float motor_audio_sample_one(motor_osc_t *m, float sample_rate) { + float rpm; + float shaft_hz; + float blade_hz; + float tip; + float load; + float asym; + float tonal; + float n; + float air; + float buzz; + float env; + float s; + float dt_blade; + float dt_shaft; + + rpm = m->rpm_smooth; + if (rpm < 80.0f) + return 0.0f; + + /* Two-blade prop: shaft = rpm/60, blade-pass = 2 * shaft */ + shaft_hz = (rpm / 60.0f) * m->detune; + blade_hz = shaft_hz * 2.0f; + if (blade_hz > sample_rate * 0.42f) + blade_hz = sample_rate * 0.42f; + + /* Normalized tip-speed proxy for loudness (~0..1 around normal FPV RPM). */ + tip = clampf(rpm / 38000.0f, 0.0f, 1.35f); + tip = tip * tip; /* closer to acoustic power ~ tip^2..tip^4 */ + + /* --- Loading / thickness: sharp blade-pass pulse via harmonic series --- */ + { + float p = m->phase; + float h1 = sinf(p); + float h2 = 0.62f * sinf(p * 2.0f); + float h3 = 0.38f * sinf(p * 3.0f); + float h4 = 0.22f * sinf(p * 4.0f); + float h5 = 0.12f * sinf(p * 5.0f); + /* Soft pulse peaking once per blade pass (more "prop" than sine). */ + float pulse = 0.55f + 0.45f * h1; + pulse = pulse * pulse; + load = (h1 + h2 + h3 + h4 + h5) * pulse; + } + + /* Blade asymmetry / imbalance at shaft rate (one blade slightly louder). */ + asym = 1.0f + 0.12f * sinf(m->shaft_phase); + + tonal = load * asym * (0.55f + 0.45f * tip); + + /* --- Broadband "air" / vortex noise, amplitude-modulated at BPF --- */ + n = frand(&m->rng); + /* Mild band-limit: one-pole LP ~3–6 kHz depending on tip speed */ + { + float lp_a = 0.12f + 0.18f * tip; + m->noise_lp += (n - m->noise_lp) * lp_a; + m->noise_hp += (m->noise_lp - m->noise_hp) * 0.02f; /* remove rumble DC */ + air = m->noise_lp - m->noise_hp; + } + /* Classic propeller AM of self-noise at blade-pass */ + env = 0.55f + 0.45f * (0.5f + 0.5f * sinf(m->phase)); + air *= env * (0.22f + 0.55f * tip); + + /* --- Weak motor electrical buzz (~ poles/2 electrical cycles; approx) --- */ + { + float buzz_hz = shaft_hz * 7.0f; /* ~14-pole motor electrical-ish */ + if (buzz_hz > sample_rate * 0.4f) + buzz_hz = sample_rate * 0.4f; + buzz = 0.08f * tip * sinf(m->buzz_phase); + buzz += 0.04f * tip * sinf(m->buzz_phase * 2.0f); + m->buzz_phase += (float)(2.0 * M_PI) * (buzz_hz / sample_rate); + if (m->buzz_phase >= (float)(2.0 * M_PI)) + m->buzz_phase -= (float)(2.0 * M_PI); + } + + s = tonal * 0.78f + air * 0.55f + buzz; + + /* Advance continuous phases */ + dt_blade = (float)(2.0 * M_PI) * (blade_hz / sample_rate); + dt_shaft = (float)(2.0 * M_PI) * (shaft_hz / sample_rate); + m->phase += dt_blade; + if (m->phase >= (float)(2.0 * M_PI)) + m->phase -= (float)(2.0 * M_PI); + m->shaft_phase += dt_shaft; + if (m->shaft_phase >= (float)(2.0 * M_PI)) + m->shaft_phase -= (float)(2.0 * M_PI); + + /* Idle is quieter / thinner */ + { + float idle = clampf((rpm - 1500.0f) / 6000.0f, 0.15f, 1.0f); + s *= idle; + } + + return s; +} + +void motor_audio_render(int16_t *output, uint32_t frame_count, uint32_t sample_rate, uint32_t timestamp_ms) { + uint32_t i, m; + float alpha; + float gain_target; + float gain_alpha; + float left, right; + int32_t pcm_l, pcm_r; + uint32_t now_ms; + float mix_lp_a; + + if (!output || frame_count == 0 || sample_rate == 0) + return; + + if (!g_enabled) { + memset(output, 0, (size_t)frame_count * MOTOR_AUDIO_CHANNELS * sizeof(int16_t)); + if (timestamp_ms != 0) + g_render_ms = timestamp_ms + (frame_count * 1000u) / sample_rate; + else + g_render_ms += (frame_count * 1000u) / sample_rate; + return; + } + + if (timestamp_ms != 0) + now_ms = timestamp_ms; + else + now_ms = g_render_ms; + + alpha = 1.0f - expf(-1.0f / ((float)sample_rate * ((float)MOTOR_AUDIO_SMOOTH_MS / 1000.0f))); + gain_alpha = 1.0f - expf(-1.0f / ((float)sample_rate * 0.05f)); + /* Cabin / soft distance: ~6–8 kHz two-pole LP */ + mix_lp_a = 1.0f - expf(-2.0f * (float)M_PI * 7000.0f / (float)sample_rate); + + for (i = 0; i < frame_count; i++) { + uint32_t sample_ms = now_ms + (i * 1000u) / sample_rate; + + gain_target = 0.0f; + if (g_have_rpm) { + uint32_t age = sample_ms - g_last_rpm_ms; + if ((int32_t)age < 0) + age = 0; + if (age <= MOTOR_AUDIO_STALE_MS) { + float any = 0.0f; + for (m = 0; m < MOTOR_AUDIO_MOTORS; m++) + any += g_motors[m].rpm_target; + if (any > 0.5f) + gain_target = 1.0f; + } + } + + g_master_gain += (gain_target - g_master_gain) * gain_alpha; + + left = 0.0f; + right = 0.0f; + for (m = 0; m < MOTOR_AUDIO_MOTORS; m++) { + float s; + g_motors[m].rpm_smooth += (g_motors[m].rpm_target - g_motors[m].rpm_smooth) * alpha; + s = motor_audio_sample_one(&g_motors[m], (float)sample_rate); + s *= g_front_rear[m]; + left += s * g_pan_l[m]; + right += s * g_pan_r[m]; + } + + left = softsat(left * (0.26f * g_master_gain)); + right = softsat(right * (0.26f * g_master_gain)); + + g_mix_lp_l += (left - g_mix_lp_l) * mix_lp_a; + g_mix_lp_l2 += (g_mix_lp_l - g_mix_lp_l2) * mix_lp_a; + g_mix_lp_r += (right - g_mix_lp_r) * mix_lp_a; + g_mix_lp_r2 += (g_mix_lp_r - g_mix_lp_r2) * mix_lp_a; + left = g_mix_lp_l2; + right = g_mix_lp_r2; + + pcm_l = (int32_t)lrintf(left * 28000.0f); + pcm_r = (int32_t)lrintf(right * 28000.0f); + if (pcm_l > 32767) + pcm_l = 32767; + if (pcm_l < -32768) + pcm_l = -32768; + if (pcm_r > 32767) + pcm_r = 32767; + if (pcm_r < -32768) + pcm_r = -32768; + output[i * 2] = (int16_t)pcm_l; + output[i * 2 + 1] = (int16_t)pcm_r; + + { + int16_t al = (int16_t)(pcm_l < 0 ? -pcm_l : pcm_l); + int16_t ar = (int16_t)(pcm_r < 0 ? -pcm_r : pcm_r); + if (al > g_peak_abs) + g_peak_abs = al; + if (ar > g_peak_abs) + g_peak_abs = ar; + } + } + + g_render_ms = now_ms + (frame_count * 1000u) / sample_rate; +} diff --git a/src/core/motor_audio.h b/src/core/motor_audio.h new file mode 100644 index 000000000..cf1e29e86 --- /dev/null +++ b/src/core/motor_audio.h @@ -0,0 +1,62 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* + * Synthetic motor / propeller audio from mechanical RPM telemetry. + * + * Platform-independent PCM generator. Emulator uses SDL2/WAV backends; + * hardware goggles currently have no general-purpose PCM playback path + * (beep.c is GPIO only; adec2ao is Softwinner media decode). Wire this + * module to a real AO/PCM sink when one is available. + */ + +#define MOTOR_AUDIO_MOTORS 4 +#define MOTOR_AUDIO_CHANNELS 2 /* stereo L/R interleaved */ +/* 22.05 kHz is enough for blade-pass + a few harmonics (~<8 kHz); halves CPU vs 48 kHz. */ +#define MOTOR_AUDIO_DEFAULT_RATE 22050 +#define MOTOR_AUDIO_STALE_MS 500 +#define MOTOR_AUDIO_SMOOTH_MS 40 + +/* + * Betaflight QUADX motor order (nose forward): + * 4 (FL) 2 (FR) + * 3 (RL) 1 (RR) + * Front motors are louder; left/right are panned in stereo. + */ + +void motor_audio_reset(void); + +/* Menu / settings gate. When disabled, render() outputs silence (RPM still updates). */ +void motor_audio_set_enabled(int enabled); +int motor_audio_is_enabled(void); + +/* + * Optional clock override for offline replay/tests. When set, motor_audio_now_ms() + * returns (*fn)() instead of wall time. Pass NULL to restore wall clock. + */ +void motor_audio_set_time_fn(uint32_t (*fn)(void)); +uint32_t motor_audio_now_ms(void); +/* Internal PCM timeline used when render(..., timestamp_ms=0). */ +uint32_t motor_audio_render_clock_ms(void); + +/* Update target RPM for four motors. timestamp_ms is a monotonic clock. */ +void motor_audio_set_rpm(uint32_t rpm0, uint32_t rpm1, uint32_t rpm2, uint32_t rpm3, uint32_t timestamp_ms); + +/* + * Render stereo int16 PCM, interleaved L,R into output[frame_count * 2]. + * sample_rate typically MOTOR_AUDIO_DEFAULT_RATE. Uses an internal render clock advanced by + * frame_count; pass timestamp_ms for stale-fade decisions (0 = use internal). + */ +void motor_audio_render(int16_t *output, uint32_t frame_count, uint32_t sample_rate, uint32_t timestamp_ms); + +/* Peak absolute sample seen since last reset (for tests). */ +int16_t motor_audio_peak_abs(void); + +#ifdef __cplusplus +} +#endif diff --git a/src/core/motor_rpm_proto.c b/src/core/motor_rpm_proto.c new file mode 100644 index 000000000..c542a67bd --- /dev/null +++ b/src/core/motor_rpm_proto.c @@ -0,0 +1,56 @@ +#include "motor_rpm_proto.h" + +uint16_t motor_rpm_quantize(uint32_t rpm) { + uint32_t q = (rpm + 16u) >> 5; + if (q > MOTOR_RPM_Q_MAX) + q = MOTOR_RPM_Q_MAX; + return (uint16_t)q; +} + +uint32_t motor_rpm_dequantize(uint16_t q) { + if (q > MOTOR_RPM_Q_MAX) + q = MOTOR_RPM_Q_MAX; + return (uint32_t)q * MOTOR_RPM_STEP; +} + +void motor_rpm_pack(const uint16_t q_in[MOTOR_RPM_COUNT], uint8_t out[MOTOR_RPM_PACKED_BYTES]) { + uint16_t q0 = q_in[0] & MOTOR_RPM_Q_MAX; + uint16_t q1 = q_in[1] & MOTOR_RPM_Q_MAX; + uint16_t q2 = q_in[2] & MOTOR_RPM_Q_MAX; + uint16_t q3 = q_in[3] & MOTOR_RPM_Q_MAX; + + out[0] = (uint8_t)(q0 & 0xff); + out[1] = (uint8_t)(((q0 >> 8) & 0x0f) | ((q1 & 0x0f) << 4)); + out[2] = (uint8_t)((q1 >> 4) & 0xff); + out[3] = (uint8_t)(q2 & 0xff); + out[4] = (uint8_t)(((q2 >> 8) & 0x0f) | ((q3 & 0x0f) << 4)); + out[5] = (uint8_t)((q3 >> 4) & 0xff); +} + +void motor_rpm_unpack(const uint8_t in[MOTOR_RPM_PACKED_BYTES], uint16_t q[MOTOR_RPM_COUNT]) { + q[0] = (uint16_t)(in[0] | ((in[1] & 0x0f) << 8)); + q[1] = (uint16_t)(((in[1] >> 4) & 0x0f) | (in[2] << 4)); + q[2] = (uint16_t)(in[3] | ((in[4] & 0x0f) << 8)); + q[3] = (uint16_t)(((in[4] >> 4) & 0x0f) | (in[5] << 4)); +} + +int motor_rpm_from_msp_telemetry(const uint8_t *payload, uint8_t len, uint32_t rpm[MOTOR_RPM_COUNT]) { + uint8_t i; + uint8_t base; + + if (!payload || !rpm) + return 0; + if (len < 53) + return 0; + if (payload[0] < 4) + return 0; + + for (i = 0; i < MOTOR_RPM_COUNT; i++) { + base = 1 + (i * 13); + rpm[i] = (uint32_t)payload[base] | + ((uint32_t)payload[base + 1] << 8) | + ((uint32_t)payload[base + 2] << 16) | + ((uint32_t)payload[base + 3] << 24); + } + return 1; +} diff --git a/src/core/motor_rpm_proto.h b/src/core/motor_rpm_proto.h new file mode 100644 index 000000000..103adebdb --- /dev/null +++ b/src/core/motor_rpm_proto.h @@ -0,0 +1,29 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define MOTOR_RPM_COUNT 4 +#define MOTOR_RPM_Q_BITS 12 +#define MOTOR_RPM_Q_MAX 4095 +#define MOTOR_RPM_STEP 32 +#define MOTOR_RPM_MAX_REPR 131040u /* 4095 * 32 */ +#define MOTOR_RPM_PACKED_BYTES 6 +#define MOTOR_RPM_SERVICE_LEGACY 15 +#define MOTOR_RPM_SERVICE_LEN 21 /* legacy 15 + 6 RPM */ +#define MOTOR_RPM_SERVICE_OFF 16 /* first RPM byte in VRX rx_buf */ + +uint16_t motor_rpm_quantize(uint32_t rpm); +uint32_t motor_rpm_dequantize(uint16_t q); +void motor_rpm_pack(const uint16_t q[MOTOR_RPM_COUNT], uint8_t out[MOTOR_RPM_PACKED_BYTES]); +void motor_rpm_unpack(const uint8_t in[MOTOR_RPM_PACKED_BYTES], uint16_t q[MOTOR_RPM_COUNT]); + +/* MSP_MOTOR_TELEMETRY payload → motors 0..3. Returns 1 on success. */ +int motor_rpm_from_msp_telemetry(const uint8_t *payload, uint8_t len, uint32_t rpm[MOTOR_RPM_COUNT]); + +#ifdef __cplusplus +} +#endif diff --git a/src/core/msp_displayport.c b/src/core/msp_displayport.c index 46bd99b9f..803343298 100644 --- a/src/core/msp_displayport.c +++ b/src/core/msp_displayport.c @@ -4,7 +4,14 @@ #include #include +#include "motor_audio.h" +#include "motor_rpm_proto.h" +#ifdef MOTOR_AUDIO_POC_HOST +void load_fc_osd_font(uint8_t fhd); +void osd_signal_update(void); +#else #include "osd.h" +#endif #include "util/time.h" uint8_t crc8tab[256] = { @@ -357,6 +364,18 @@ void parser_config(uint8_t *rx_buf) { vtxTypeDetect(rx_buf[10]); vtxFcLockDetect(rx_buf[11]); vtxCamRatioDetect(rx_buf[12]); + + /* Extended 0xFF service: length 21 includes 6 packed RPM bytes at [16..21]. */ + if (rx_buf[0] >= MOTOR_RPM_SERVICE_LEN) { + uint16_t q[MOTOR_RPM_COUNT]; + uint32_t rpm[MOTOR_RPM_COUNT]; + int i; + + motor_rpm_unpack(&rx_buf[MOTOR_RPM_SERVICE_OFF], q); + for (i = 0; i < MOTOR_RPM_COUNT; i++) + rpm[i] = motor_rpm_dequantize(q[i]); + motor_audio_set_rpm(rpm[0], rpm[1], rpm[2], rpm[3], motor_audio_now_ms()); + } } /* diff --git a/src/core/msp_displayport.h b/src/core/msp_displayport.h index 881de4fb7..5582ef6b3 100644 --- a/src/core/msp_displayport.h +++ b/src/core/msp_displayport.h @@ -83,6 +83,9 @@ extern uint8_t osd_page_buf[HD_VMAX][7]; extern uint16_t last_rcv_seconds0; extern uint16_t last_rcv_seconds1; +/* Shared HDZero displayport CRC table (also used by motor-audio replay tooling). */ +extern uint8_t crc8tab[256]; + #ifdef __cplusplus } #endif diff --git a/src/core/settings.c b/src/core/settings.c index b2245972c..bde6eebf2 100644 --- a/src/core/settings.c +++ b/src/core/settings.c @@ -52,6 +52,7 @@ const setting_t g_setting_defaults = { .osd = true, .audio = true, .audio_source = SETTING_RECORD_AUDIO_SOURCE_MIC, + .motor_audio = false, .naming = SETTING_NAMING_CONTIGUOUS, }, .image = { @@ -424,6 +425,7 @@ void settings_load(void) { g_setting.record.osd = settings_get_bool("record", "osd", g_setting_defaults.record.osd); g_setting.record.audio = settings_get_bool("record", "audio", g_setting_defaults.record.audio); g_setting.record.audio_source = ini_getl("record", "audio_source", g_setting_defaults.record.audio_source, SETTING_INI); + g_setting.record.motor_audio = settings_get_bool("record", "motor_audio", g_setting_defaults.record.motor_audio); g_setting.record.naming = ini_getl("record", "naming", g_setting_defaults.record.naming, SETTING_INI); // image diff --git a/src/core/settings.h b/src/core/settings.h index 3965a2bf8..1d64937a7 100644 --- a/src/core/settings.h +++ b/src/core/settings.h @@ -102,6 +102,8 @@ typedef struct { bool osd; bool audio; setting_record_audio_source_t audio_source; + /* Mix synthetic motor sound into the selected audio source (headphones/line-out). */ + bool motor_audio; setting_record_naming_t naming; } setting_record_t; diff --git a/src/emulator/motor_audio_dev.c b/src/emulator/motor_audio_dev.c new file mode 100644 index 000000000..83a5599d9 --- /dev/null +++ b/src/emulator/motor_audio_dev.c @@ -0,0 +1,766 @@ +#include "motor_audio_dev.h" + +#include "core/motor_audio.h" +#include "core/motor_rpm_proto.h" +#include "core/msp_displayport.h" +#include "motor_audio_sdl.h" +#include "util/time.h" + +#include +#include +#include +#include +#include + +#ifdef MOTOR_AUDIO_POC_HOST +float sinf(float); +float cosf(float); +#else +#include +#endif + +#define SAMPLE_RATE MOTOR_AUDIO_DEFAULT_RATE +#define TELEMETRY_PERIOD_MS 125 + +typedef struct { + int32_t time_us; + int32_t erpm[4]; +} bbl_sample_t; + +typedef struct { + FILE *fp; + uint32_t sample_rate; + uint32_t data_bytes; +} wav_writer_t; + +static int g_drop_every; +static uint32_t g_pkt_count; +static uint32_t g_pkt_dropped; +static uint32_t g_clip_count; + +static int wav_open(wav_writer_t *w, const char *path, uint32_t sample_rate) { + memset(w, 0, sizeof(*w)); + w->fp = fopen(path, "wb"); + if (!w->fp) + return -1; + w->sample_rate = sample_rate; + /* Placeholder header; rewritten on close. */ + uint8_t hdr[44] = {0}; + if (fwrite(hdr, 1, 44, w->fp) != 44) + return -1; + return 0; +} + +static int wav_write(wav_writer_t *w, const int16_t *samples, uint32_t count) { + size_t n = fwrite(samples, sizeof(int16_t), count, w->fp); + if (n != count) + return -1; + w->data_bytes += (uint32_t)(n * sizeof(int16_t)); + return 0; +} + +static int wav_close(wav_writer_t *w) { + uint32_t sr = w->sample_rate; + uint16_t channels = MOTOR_AUDIO_CHANNELS; + uint16_t block_align = (uint16_t)(channels * 2); + uint32_t byte_rate = sr * block_align; + uint32_t data_sz = w->data_bytes; + uint32_t riff_sz = 36 + data_sz; + uint8_t hdr[44]; + + if (!w->fp) + return -1; + + memcpy(hdr + 0, "RIFF", 4); + hdr[4] = (uint8_t)(riff_sz); + hdr[5] = (uint8_t)(riff_sz >> 8); + hdr[6] = (uint8_t)(riff_sz >> 16); + hdr[7] = (uint8_t)(riff_sz >> 24); + memcpy(hdr + 8, "WAVEfmt ", 8); + hdr[16] = 16; + hdr[17] = 0; + hdr[18] = 0; + hdr[19] = 0; /* fmt chunk size */ + hdr[20] = 1; + hdr[21] = 0; /* PCM */ + hdr[22] = (uint8_t)channels; + hdr[23] = 0; + hdr[24] = (uint8_t)(sr); + hdr[25] = (uint8_t)(sr >> 8); + hdr[26] = (uint8_t)(sr >> 16); + hdr[27] = (uint8_t)(sr >> 24); + hdr[28] = (uint8_t)(byte_rate); + hdr[29] = (uint8_t)(byte_rate >> 8); + hdr[30] = (uint8_t)(byte_rate >> 16); + hdr[31] = (uint8_t)(byte_rate >> 24); + hdr[32] = (uint8_t)block_align; + hdr[33] = 0; + hdr[34] = 16; + hdr[35] = 0; /* bits */ + memcpy(hdr + 36, "data", 4); + hdr[40] = (uint8_t)(data_sz); + hdr[41] = (uint8_t)(data_sz >> 8); + hdr[42] = (uint8_t)(data_sz >> 16); + hdr[43] = (uint8_t)(data_sz >> 24); + + if (fseek(w->fp, 0, SEEK_SET) != 0) + return -1; + if (fwrite(hdr, 1, 44, w->fp) != 44) + return -1; + fclose(w->fp); + w->fp = NULL; + return 0; +} + +/* Build a complete HDZero 0xFF service frame with optional 6-byte RPM tail. */ +static uint8_t build_service_frame(uint8_t *frame, const uint32_t rpm[4], int with_rpm, uint8_t lq) { + uint8_t payload_len = with_rpm ? MOTOR_RPM_SERVICE_LEN : MOTOR_RPM_SERVICE_LEGACY; + uint8_t i; + uint8_t crc0 = 0, crc1 = 0; + uint8_t len; + + frame[0] = HEADER0; + frame[1] = HEADER1; + frame[2] = 0xff; + frame[3] = payload_len; + frame[4] = 0x99; /* 720P60 */ + frame[5] = 'B'; + frame[6] = 'T'; + frame[7] = 'F'; + frame[8] = 'L'; + frame[9] = lq; + frame[10] = 0; + frame[11] = 0; + frame[12] = 0; + frame[13] = 0; + frame[14] = 0x03; + frame[15] = 0x55; + frame[16] = 1; + frame[17] = 0; + frame[18] = 0; + + if (with_rpm) { + uint16_t q[4]; + for (i = 0; i < 4; i++) { + q[i] = motor_rpm_quantize(rpm[i]); + if (rpm[i] > MOTOR_RPM_MAX_REPR) + g_clip_count++; + } + motor_rpm_pack(q, &frame[19]); + } + + len = (uint8_t)(4 + payload_len); /* header+index+len + payload */ + for (i = 0; i < len; i++) { + crc0 ^= frame[i]; + crc1 = crc8tab[crc1 ^ frame[i]]; + } + frame[len] = crc0; + frame[len + 1] = crc1; + return (uint8_t)(len + 2); +} + +static void feed_service_rpm(const uint32_t rpm[4], int with_rpm, uint8_t lq) { + uint8_t frame[64]; + uint8_t n; + + g_pkt_count++; + if (g_drop_every > 0 && (g_pkt_count % (uint32_t)g_drop_every) == 0) { + g_pkt_dropped++; + return; + } + + n = build_service_frame(frame, rpm, with_rpm, lq); + recive_one_frame(frame, n); +} + +static int write_pcm_seconds(wav_writer_t *wav, float seconds, int use_sdl_realtime) { + uint32_t total = (uint32_t)(seconds * SAMPLE_RATE); + uint32_t chunk = 1024; + int16_t buf[1024 * MOTOR_AUDIO_CHANNELS]; + uint32_t done = 0; + + while (done < total) { + uint32_t n = total - done; + if (n > chunk) + n = chunk; + motor_audio_render(buf, n, SAMPLE_RATE, 0); + if (wav && wav->fp) { + if (wav_write(wav, buf, n * MOTOR_AUDIO_CHANNELS) != 0) + return -1; + } + if (use_sdl_realtime) { + /* SDL callback also renders; for realtime replay we sleep wall time. */ + usleep((useconds_t)((n * 1000000ull) / SAMPLE_RATE)); + } + done += n; + } + return 0; +} + +/* ---------------- unit tests ---------------- */ + +static int expect_true(int cond, const char *msg) { + if (!cond) { + fprintf(stderr, "FAIL: %s\n", msg); + return 1; + } + return 0; +} + +static int test_roundtrip_rpm(uint32_t rpm, uint32_t max_err) { + uint16_t q = motor_rpm_quantize(rpm); + uint32_t back = motor_rpm_dequantize(q); + uint32_t err = (back > rpm) ? (back - rpm) : (rpm - back); + if (rpm > MOTOR_RPM_MAX_REPR) { + if (q != MOTOR_RPM_Q_MAX || back != MOTOR_RPM_MAX_REPR) + return 1; + return 0; + } + return err > max_err; +} + +static int run_unit_tests(void) { + int fails = 0; + uint16_t q[4], q2[4]; + uint8_t packed[6]; + uint32_t rpm[4]; + uint8_t msp[53]; + uint8_t frame[64]; + uint8_t n; + int16_t *pcm; + int i; + + pcm = (int16_t *)malloc(SAMPLE_RATE * MOTOR_AUDIO_CHANNELS * sizeof(int16_t)); + if (!pcm) { + fprintf(stderr, "oom\n"); + return 1; + } + fails += expect_true(test_roundtrip_rpm(0, 0) == 0, "RPM 0 round-trip"); + fails += expect_true(test_roundtrip_rpm(3200, 16) == 0, "RPM 3200 within 16"); + fails += expect_true(test_roundtrip_rpm(25000, 16) == 0, "RPM 25000 within 16"); + fails += expect_true(test_roundtrip_rpm(45000, 16) == 0, "RPM 45000 within 16"); + fails += expect_true(test_roundtrip_rpm(MOTOR_RPM_MAX_REPR, 0) == 0, "max 131040 round-trip"); + fails += expect_true(test_roundtrip_rpm(200000, 0) == 0, "above max clamps"); + q[0] = 0; + q[1] = 100; + q[2] = 2048; + q[3] = 4095; + motor_rpm_pack(q, packed); + motor_rpm_unpack(packed, q2); + fails += expect_true(q[0] == q2[0] && q[1] == q2[1] && q[2] == q2[2] && q[3] == q2[3], + "12-bit pack/unpack exact"); + + /* Golden vector: q = {1, 16, 256, 4095} */ + q[0] = 1; + q[1] = 16; + q[2] = 256; + q[3] = 4095; + motor_rpm_pack(q, packed); + fails += expect_true(packed[0] == 0x01 && packed[1] == 0x00 && packed[2] == 0x01 && + packed[3] == 0x00 && packed[4] == 0xf1 && packed[5] == 0xff, + "golden six-byte vector"); + motor_rpm_unpack(packed, q2); + fails += expect_true(q2[0] == 1 && q2[1] == 16 && q2[2] == 256 && q2[3] == 4095, + "golden unpack"); + memset(msp, 0, sizeof(msp)); + msp[0] = 4; + for (i = 0; i < 4; i++) { + uint32_t r = 1000u * (uint32_t)(i + 1); + uint8_t *p = &msp[1 + i * 13]; + p[0] = (uint8_t)(r); + p[1] = (uint8_t)(r >> 8); + p[2] = (uint8_t)(r >> 16); + p[3] = (uint8_t)(r >> 24); + } + fails += expect_true(motor_rpm_from_msp_telemetry(msp, 53, rpm) == 1, "msp extract ok"); + fails += expect_true(rpm[0] == 1000 && rpm[1] == 2000 && rpm[2] == 3000 && rpm[3] == 4000, + "msp rpm values"); + fails += expect_true(motor_rpm_from_msp_telemetry(msp, 40, rpm) == 0, "short msp ignored"); + msp[0] = 3; + fails += expect_true(motor_rpm_from_msp_telemetry(msp, 53, rpm) == 0, "motor count < 4 ignored"); + motor_audio_reset(); + rpm[0] = rpm[1] = rpm[2] = rpm[3] = 20000; + n = build_service_frame(frame, rpm, 0, 0); + recive_one_frame(frame, n); + motor_audio_render(pcm, 4800, SAMPLE_RATE, 1000); + { + int silent = 1; + for (i = 0; i < 4800 * MOTOR_AUDIO_CHANNELS; i++) { + if (pcm[i] != 0) { + silent = 0; + break; + } + } + fails += expect_true(silent, "old 15-byte service keeps audio silent"); + } + motor_audio_reset(); + n = build_service_frame(frame, rpm, 1, 1); + recive_one_frame(frame, n); + /* Advance internal clock past smoothing; feed timestamp via render */ + motor_audio_render(pcm, 4800, SAMPLE_RATE, time_ms() ? time_ms() : 100); + { + int any = 0; + for (i = 0; i < 4800 * MOTOR_AUDIO_CHANNELS; i++) { + if (pcm[i] != 0) { + any = 1; + break; + } + } + fails += expect_true(any, "21-byte service enables audio"); + } + + /* Stale telemetry fades to silence */ + motor_audio_reset(); + motor_audio_set_rpm(30000, 30000, 30000, 30000, 0); + motor_audio_render(pcm, SAMPLE_RATE / 10, SAMPLE_RATE, 0); /* 100ms audible */ + motor_audio_render(pcm, SAMPLE_RATE, SAMPLE_RATE, 600); /* stale > 500ms */ + { + int end_silent = 1; + for (i = ((int)SAMPLE_RATE - 1000) * MOTOR_AUDIO_CHANNELS; i < (int)SAMPLE_RATE * MOTOR_AUDIO_CHANNELS; i++) { + if (pcm[i] != 0) { + end_silent = 0; + break; + } + } + fails += expect_true(end_silent, "stale telemetry fades to zero"); + } + + /* Zero RPM fades to silence */ + motor_audio_reset(); + motor_audio_set_rpm(25000, 25000, 25000, 25000, 0); + motor_audio_render(pcm, SAMPLE_RATE / 5, SAMPLE_RATE, 0); + motor_audio_set_rpm(0, 0, 0, 0, 200); + motor_audio_render(pcm, SAMPLE_RATE, SAMPLE_RATE, 200); + { + int end_silent = 1; + for (i = ((int)SAMPLE_RATE - 2000) * MOTOR_AUDIO_CHANNELS; i < (int)SAMPLE_RATE * MOTOR_AUDIO_CHANNELS; i++) { + if (pcm[i] < -50 || pcm[i] > 50) { + end_silent = 0; + break; + } + } + fails += expect_true(end_silent, "zero RPM fades to silence"); + } + fails += expect_true(motor_audio_peak_abs() <= 32767, "samples within int16 range"); + + free(pcm); + + if (fails == 0) + printf("motor_audio unit tests: PASS\n"); + else + printf("motor_audio unit tests: %d FAIL\n", fails); + return fails ? 1 : 0; +} + +/* ---------------- synthetic timeline ---------------- */ + +static void synthetic_rpm_at(float t_sec, uint32_t rpm[4]) { + float thr; + float base; + + if (t_sec < 1.0f) { + /* idle */ + base = 3000.0f; + } else if (t_sec < 4.0f) { + /* gradual throttle increase */ + thr = (t_sec - 1.0f) / 3.0f; + base = 3000.0f + thr * 22000.0f; + } else if (t_sec < 7.0f) { + /* roll/yaw maneuver: diverge motors */ + thr = (t_sec - 4.0f) / 3.0f; + base = 25000.0f; + rpm[0] = (uint32_t)(base + 8000.0f * sinf(thr * 6.0f)); + rpm[1] = (uint32_t)(base - 6000.0f * sinf(thr * 5.0f)); + rpm[2] = (uint32_t)(base + 5000.0f * cosf(thr * 4.0f)); + rpm[3] = (uint32_t)(base - 7000.0f * cosf(thr * 7.0f)); + return; + } else if (t_sec < 9.0f) { + base = 40000.0f; /* full throttle */ + } else if (t_sec < 12.0f) { + thr = (t_sec - 9.0f) / 3.0f; + base = 40000.0f - thr * 35000.0f; + } else { + base = 0.0f; /* disarm */ + } + + rpm[0] = rpm[1] = rpm[2] = rpm[3] = (uint32_t)base; +} + +static int run_synthetic(const char *wav_path, int realtime) { + wav_writer_t wav; + float t; + uint32_t rpm[4]; + uint8_t lq = 0; + int use_wav = wav_path && wav_path[0]; + + g_pkt_count = 0; + g_pkt_dropped = 0; + g_clip_count = 0; + motor_audio_reset(); + /* Offline WAV must share one clock: parser RPM stamps vs PCM render timeline. */ + motor_audio_set_time_fn(motor_audio_render_clock_ms); + + if (use_wav && wav_open(&wav, wav_path, SAMPLE_RATE) != 0) { + fprintf(stderr, "cannot open wav %s\n", wav_path); + return 1; + } + + /* Realtime SDL playback only when not writing WAV (avoid double-render). */ + if (realtime && !use_wav) { + if (motor_audio_sdl_start(SAMPLE_RATE) != 0) + fprintf(stderr, "warning: SDL audio unavailable\n"); + } + + printf("synthetic flight: idle -> throttle -> maneuver -> full -> down -> disarm\n"); + + for (t = 0.0f; t <= 14.0f; t += TELEMETRY_PERIOD_MS / 1000.0f) { + synthetic_rpm_at(t, rpm); + feed_service_rpm(rpm, 1, lq++); + if (use_wav) { + if (write_pcm_seconds(&wav, TELEMETRY_PERIOD_MS / 1000.0f, 0) != 0) + return 1; + if (realtime) + usleep(TELEMETRY_PERIOD_MS * 1000); + } else if (realtime) { + usleep(TELEMETRY_PERIOD_MS * 1000); + } else { + int16_t buf[(SAMPLE_RATE / 8) * MOTOR_AUDIO_CHANNELS]; + motor_audio_render(buf, SAMPLE_RATE / 8, SAMPLE_RATE, 0); + } + } + + if (realtime) + motor_audio_sdl_stop(); + if (use_wav) { + wav_close(&wav); + printf("wrote %s (%u packets, %u clipped)\n", wav_path, g_pkt_count, g_clip_count); + } + motor_audio_set_time_fn(NULL); + return 0; +} + +/* ---------------- blackbox CSV replay ---------------- */ + +static char *trim(char *s) { + char *e; + while (*s == ' ' || *s == '\t' || *s == '"') + s++; + e = s + strlen(s); + while (e > s && (e[-1] == '\n' || e[-1] == '\r' || e[-1] == ' ' || e[-1] == '\t' || e[-1] == '"')) + *--e = 0; + return s; +} + +static int header_col(char **cols, int n, const char *name) { + int i; + for (i = 0; i < n; i++) { + if (strcmp(cols[i], name) == 0) + return i; + } + return -1; +} + +/* + * Match eRPM[0] / eRPM(/100)[0] headers from blackbox_decode. + * *out_div100 is set when the matched column is already divided by 100. + */ +static int header_col_erpm(char **cols, int n, int motor, int *out_div100) { + char exact[32]; + char alt[40]; + int i; + + snprintf(exact, sizeof(exact), "eRPM[%d]", motor); + snprintf(alt, sizeof(alt), "eRPM(/100)[%d]", motor); + i = header_col(cols, n, exact); + if (i >= 0) { + if (out_div100) + *out_div100 = 0; + return i; + } + i = header_col(cols, n, alt); + if (i >= 0) { + if (out_div100) + *out_div100 = 1; + return i; + } + for (i = 0; i < n; i++) { + const char *p = strstr(cols[i], "eRPM"); + char bracket[8]; + if (!p) + continue; + snprintf(bracket, sizeof(bracket), "[%d]", motor); + if (!strstr(p, bracket)) + continue; + if (out_div100) + *out_div100 = (strstr(cols[i], "/100") != NULL); + return i; + } + return -1; +} + +/* mechanical RPM = eRPM * 2 / poles; blackbox may store eRPM/100. */ +static uint32_t erpm_to_mech(int32_t erpm_field, int motor_poles, int div100) { + int64_t erpm = div100 ? ((int64_t)erpm_field * 100) : (int64_t)erpm_field; + if (erpm < 0) + erpm = 0; + return (uint32_t)(erpm * 2 / motor_poles); +} + +static int split_csv_line(char *line, char **cols, int max_cols) { + int n = 0; + char *p = line; + while (n < max_cols) { + cols[n++] = p; + p = strchr(p, ','); + if (!p) + break; + *p++ = 0; + } + return n; +} + +static int run_blackbox_replay(const char *csv_path, int motor_poles, const char *wav_path, int realtime) { + FILE *fp; + char line[4096]; + char *cols[256]; + int ncols; + int idx_time = -1; + int idx_erpm[4] = {-1, -1, -1, -1}; + int erpm_div100 = 0; + bbl_sample_t *samples = NULL; + size_t sample_count = 0, sample_cap = 0; + size_t i; + int32_t t0 = 0; + uint32_t rpm_min[4], rpm_max[4]; + wav_writer_t wav; + int use_wav = wav_path && wav_path[0]; + uint8_t lq = 0; + int64_t next_tx_us; + uint32_t latest[4] = {0, 0, 0, 0}; + size_t si = 0; + + if (motor_poles < 2) { + fprintf(stderr, "error: --motor-poles N is required (N >= 2)\n"); + return 1; + } + + fp = fopen(csv_path, "r"); + if (!fp) { + fprintf(stderr, "error: cannot open %s\n", csv_path); + return 1; + } + + if (!fgets(line, sizeof(line), fp)) { + fprintf(stderr, "error: empty CSV\n"); + fclose(fp); + return 1; + } + + ncols = split_csv_line(line, cols, 256); + for (i = 0; i < (size_t)ncols; i++) + cols[i] = trim(cols[i]); + + idx_time = header_col(cols, ncols, "time"); + if (idx_time < 0) + idx_time = header_col(cols, ncols, "time (us)"); + idx_erpm[0] = header_col_erpm(cols, ncols, 0, &erpm_div100); + idx_erpm[1] = header_col_erpm(cols, ncols, 1, NULL); + idx_erpm[2] = header_col_erpm(cols, ncols, 2, NULL); + idx_erpm[3] = header_col_erpm(cols, ncols, 3, NULL); + + if (idx_time < 0 || idx_erpm[0] < 0 || idx_erpm[1] < 0 || idx_erpm[2] < 0 || idx_erpm[3] < 0) { + fprintf(stderr, + "error: CSV missing required fields (need time/time (us) and eRPM[0..3] or eRPM(/100)[0..3]).\n" + "This log was recorded without RPM fields and cannot reproduce actual motor RPM.\n"); + fclose(fp); + return 1; + } + + while (fgets(line, sizeof(line), fp)) { + char *c[256]; + int nc = split_csv_line(line, c, 256); + bbl_sample_t s; + if (nc <= idx_erpm[3]) + continue; + s.time_us = (int32_t)atoi(trim(c[idx_time])); + s.erpm[0] = atoi(trim(c[idx_erpm[0]])); + s.erpm[1] = atoi(trim(c[idx_erpm[1]])); + s.erpm[2] = atoi(trim(c[idx_erpm[2]])); + s.erpm[3] = atoi(trim(c[idx_erpm[3]])); + if (sample_count == sample_cap) { + sample_cap = sample_cap ? sample_cap * 2 : 4096; + samples = realloc(samples, sample_cap * sizeof(*samples)); + if (!samples) { + fprintf(stderr, "oom\n"); + fclose(fp); + return 1; + } + } + samples[sample_count++] = s; + } + fclose(fp); + + if (sample_count == 0) { + fprintf(stderr, "error: no CSV rows\n"); + return 1; + } + + t0 = samples[0].time_us; + for (i = 0; i < 4; i++) { + rpm_min[i] = UINT32_MAX; + rpm_max[i] = 0; + } + for (i = 0; i < sample_count; i++) { + int m; + for (m = 0; m < 4; m++) { + uint32_t mech = erpm_to_mech(samples[i].erpm[m], motor_poles, erpm_div100); + if (mech < rpm_min[m]) + rpm_min[m] = mech; + if (mech > rpm_max[m]) + rpm_max[m] = mech; + } + } + + { + double dur = (samples[sample_count - 1].time_us - t0) / 1e6; + uint32_t est_pkts = (uint32_t)(dur / 0.125) + 1; + printf("blackbox replay diagnostics:\n"); + printf(" motor poles: %d\n", motor_poles); + printf(" eRPM column scale: %s\n", erpm_div100 ? "eRPM/100" : "eRPM"); + printf(" flight duration: %.2f s\n", dur); + printf(" samples: %zu\n", sample_count); + for (i = 0; i < 4; i++) + printf(" motor%zu mechanical RPM min/max: %u / %u\n", i, rpm_min[i], rpm_max[i]); + printf(" estimated 8 Hz packets: %u\n", est_pkts); + } + + g_pkt_count = 0; + g_pkt_dropped = 0; + g_clip_count = 0; + motor_audio_reset(); + motor_audio_set_time_fn(motor_audio_render_clock_ms); + + if (use_wav && wav_open(&wav, wav_path, SAMPLE_RATE) != 0) { + fprintf(stderr, "cannot open wav %s\n", wav_path); + free(samples); + return 1; + } + if (realtime && !use_wav) { + if (motor_audio_sdl_start(SAMPLE_RATE) != 0) + fprintf(stderr, "warning: SDL audio unavailable\n"); + } + + next_tx_us = samples[0].time_us; + while (si < sample_count || next_tx_us <= samples[sample_count - 1].time_us) { + while (si < sample_count && samples[si].time_us <= next_tx_us) { + int m; + for (m = 0; m < 4; m++) + latest[m] = erpm_to_mech(samples[si].erpm[m], motor_poles, erpm_div100); + si++; + } + feed_service_rpm(latest, 1, lq++); + if (use_wav) { + if (write_pcm_seconds(&wav, TELEMETRY_PERIOD_MS / 1000.0f, 0) != 0) { + free(samples); + return 1; + } + if (realtime) + usleep(TELEMETRY_PERIOD_MS * 1000); + } else if (realtime) { + usleep(TELEMETRY_PERIOD_MS * 1000); + } else { + int16_t buf[(SAMPLE_RATE / 8) * MOTOR_AUDIO_CHANNELS]; + motor_audio_render(buf, SAMPLE_RATE / 8, SAMPLE_RATE, 0); + } + next_tx_us += TELEMETRY_PERIOD_MS * 1000; + if (si >= sample_count && next_tx_us > samples[sample_count - 1].time_us + TELEMETRY_PERIOD_MS * 1000) + break; + } + + printf(" generated 8 Hz packets: %u (dropped %u)\n", g_pkt_count, g_pkt_dropped); + printf(" values clipped at 131040 RPM: %u\n", g_clip_count); + + if (realtime) + motor_audio_sdl_stop(); + if (use_wav) { + wav_close(&wav); + printf("wrote %s\n", wav_path); + } + motor_audio_set_time_fn(NULL); + free(samples); + return 0; +} + +static void print_usage(const char *argv0) { + printf("Motor audio PoC (emulator/dev):\n"); + printf(" %s --motor-test\n", argv0); + printf(" %s --motor-synthetic [--motor-wav out.wav] [--motor-realtime]\n", argv0); + printf(" %s --motor-replay --motor-poles [--motor-wav out.wav]\n", argv0); + printf(" [--motor-realtime] [--motor-drop-every N]\n"); +} + +int motor_audio_dev_main(int argc, char **argv) { + const char *replay = NULL; + const char *wav = NULL; + int poles = 0; + int do_test = 0; + int do_synth = 0; + int realtime = 0; + int i; + + if (argc < 2) + return 0; + + /* So diagnostics show up immediately when stdout is redirected. */ + setvbuf(stdout, NULL, _IOLBF, 0); + setvbuf(stderr, NULL, _IOLBF, 0); + + for (i = 1; i < argc; i++) { + if (strcmp(argv[i], "--motor-test") == 0) { + do_test = 1; + } else if (strcmp(argv[i], "--motor-synthetic") == 0) { + do_synth = 1; + } else if (strcmp(argv[i], "--motor-replay") == 0 && i + 1 < argc) { + replay = argv[++i]; + } else if (strcmp(argv[i], "--motor-poles") == 0 && i + 1 < argc) { + poles = atoi(argv[++i]); + } else if (strcmp(argv[i], "--motor-wav") == 0 && i + 1 < argc) { + wav = argv[++i]; + } else if (strcmp(argv[i], "--motor-realtime") == 0) { + realtime = 1; + } else if (strcmp(argv[i], "--motor-drop-every") == 0 && i + 1 < argc) { + g_drop_every = atoi(argv[++i]); + } else if (strcmp(argv[i], "--motor-help") == 0) { + print_usage(argv[0]); + return 1; + } else if (strncmp(argv[i], "--motor-", 8) == 0) { + fprintf(stderr, "unknown motor option: %s\n", argv[i]); + print_usage(argv[0]); + return 1; + } else { + /* not a motor-audio option; let normal emulator run */ + return 0; + } + } + + if (!do_test && !do_synth && !replay) + return 0; + + if (do_test) { + int rc = run_unit_tests(); + if (rc) + exit(rc); + } + if (do_synth) { + const char *out = wav ? wav : "tmp/motor_synthetic.wav"; + int rc = run_synthetic(out, realtime); + if (rc) + exit(rc); + } + if (replay) { + int rc = run_blackbox_replay(replay, poles, wav, realtime); + exit(rc); + } + + exit(0); +} diff --git a/src/emulator/motor_audio_dev.h b/src/emulator/motor_audio_dev.h new file mode 100644 index 000000000..77e624cd9 --- /dev/null +++ b/src/emulator/motor_audio_dev.h @@ -0,0 +1,15 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Emulator/dev entry for synthetic motor audio PoC. + * Returns 1 if argv was handled (caller should exit), 0 otherwise. + */ +int motor_audio_dev_main(int argc, char **argv); + +#ifdef __cplusplus +} +#endif diff --git a/src/emulator/motor_audio_sdl.c b/src/emulator/motor_audio_sdl.c new file mode 100644 index 000000000..872fdf807 --- /dev/null +++ b/src/emulator/motor_audio_sdl.c @@ -0,0 +1,63 @@ +#include "motor_audio_sdl.h" + +#include "core/motor_audio.h" +#include "core/SDLaccess.h" + +#include +#include + +static SDL_AudioDeviceID g_dev; +static uint32_t g_rate; + +static void motor_audio_sdl_callback(void *userdata, Uint8 *stream, int len) { + int16_t *out = (int16_t *)stream; + uint32_t frames = (uint32_t)(len / (int)(sizeof(int16_t) * MOTOR_AUDIO_CHANNELS)); + (void)userdata; + motor_audio_render(out, frames, g_rate, 0); +} + +int motor_audio_sdl_start(uint32_t sample_rate) { + SDL_AudioSpec want, have; + + if (g_dev) + return 0; + + if (sample_rate == 0) + sample_rate = MOTOR_AUDIO_DEFAULT_RATE; + g_rate = sample_rate; + + if (SDL_WasInit(SDL_INIT_AUDIO) == 0) { + if (SDL_InitSubSystem(SDL_INIT_AUDIO) != 0) { + fprintf(stderr, "motor_audio_sdl: SDL_InitSubSystem AUDIO failed: %s\n", SDL_GetError()); + return -1; + } + } + + memset(&want, 0, sizeof(want)); + want.freq = (int)sample_rate; + want.format = AUDIO_S16SYS; + want.channels = MOTOR_AUDIO_CHANNELS; + want.samples = 1024; + want.callback = motor_audio_sdl_callback; + + g_dev = SDL_OpenAudioDevice(NULL, 0, &want, &have, 0); + if (!g_dev) { + fprintf(stderr, "motor_audio_sdl: OpenAudioDevice failed: %s\n", SDL_GetError()); + return -1; + } + + motor_audio_reset(); + SDL_PauseAudioDevice(g_dev, 0); + return 0; +} + +void motor_audio_sdl_stop(void) { + if (!g_dev) + return; + SDL_CloseAudioDevice(g_dev); + g_dev = 0; +} + +int motor_audio_sdl_is_running(void) { + return g_dev != 0; +} diff --git a/src/emulator/motor_audio_sdl.h b/src/emulator/motor_audio_sdl.h new file mode 100644 index 000000000..27d49b1da --- /dev/null +++ b/src/emulator/motor_audio_sdl.h @@ -0,0 +1,16 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* SDL2 audio output feeding the shared motor_audio renderer. Emulator only. */ +int motor_audio_sdl_start(uint32_t sample_rate); +void motor_audio_sdl_stop(void); +int motor_audio_sdl_is_running(void); + +#ifdef __cplusplus +} +#endif diff --git a/src/ui/page_record.c b/src/ui/page_record.c index ebc1fb7c6..fb47b7a50 100644 --- a/src/ui/page_record.c +++ b/src/ui/page_record.c @@ -8,22 +8,46 @@ #include "../conf/ui.h" #include "../core/common.hh" +#include "core/dvr.h" +#include "core/motor_audio.h" +#include "core/settings.h" #include "driver/rtc.h" #include "lang/language.h" #include "page_common.h" #include "ui/ui_style.h" +#ifdef EMULATOR_BUILD +#include "emulator/motor_audio_sdl.h" +#endif + static btn_group_t btn_group_record_mode; static btn_group_t btn_group_format; static btn_group_t btn_group_bitrate_scale; static btn_group_t btn_group_record_osd; static btn_group_t btn_group_record_audio; static btn_group_t btn_group_audio_source; +static btn_group_t btn_group_motor_audio; static btn_group_t btn_group_file_naming; static lv_coord_t col_dsc[] = {UI_RECORD_COLS}; static lv_coord_t row_dsc[] = {UI_RECORD_ROWS}; +static void apply_motor_audio_setting(bool enabled) { + g_setting.record.motor_audio = enabled; + settings_put_bool("record", "motor_audio", enabled); + motor_audio_set_enabled(enabled ? 1 : 0); + /* Keep current Mic/Line/AV source; only open/close DAC mix path. */ + dvr_enable_line_out(true); +#ifdef EMULATOR_BUILD + if (enabled) { + if (!motor_audio_sdl_is_running()) + motor_audio_sdl_start(MOTOR_AUDIO_DEFAULT_RATE); + } else { + motor_audio_sdl_stop(); + } +#endif +} + static void update_visibility() { btn_group_enable(&btn_group_audio_source, btn_group_record_audio.current == 0); @@ -33,12 +57,13 @@ static void update_visibility() { lv_obj_clear_flag(pp_record.p_arr.panel[5], FLAG_SELECTABLE); } + /* Motor Audio is independent of Record Audio (live headphone mix). */ btn_group_enable(&btn_group_file_naming, rtc_has_battery() == 0); if (rtc_has_battery() == 0) { - lv_obj_add_flag(pp_record.p_arr.panel[6], FLAG_SELECTABLE); + lv_obj_add_flag(pp_record.p_arr.panel[7], FLAG_SELECTABLE); } else { - lv_obj_clear_flag(pp_record.p_arr.panel[6], FLAG_SELECTABLE); + lv_obj_clear_flag(pp_record.p_arr.panel[7], FLAG_SELECTABLE); } } @@ -74,9 +99,10 @@ static lv_obj_t *page_record_create(lv_obj_t *parent, panel_arr_t *arr) { create_btn_group_item(&btn_group_record_osd, cont, 2, _lang("Record OSD"), _lang("Yes"), _lang("No"), "", "", 3); create_btn_group_item(&btn_group_record_audio, cont, 2, _lang("Record Audio"), _lang("Yes"), _lang("No"), "", "", 4); create_btn_group_item(&btn_group_audio_source, cont, 3, _lang("Audio Source"), _lang("Mic"), _lang("Line In"), _lang("A/V In"), "", 5); - create_btn_group_item(&btn_group_file_naming, cont, 2, _lang("Naming Scheme"), _lang("Digits"), _lang("Date"), "", "", 6); + create_btn_group_item(&btn_group_motor_audio, cont, 2, _lang("Motor Audio"), _lang("On"), _lang("Off"), "", "", 6); + create_btn_group_item(&btn_group_file_naming, cont, 2, _lang("Naming Scheme"), _lang("Digits"), _lang("Date"), "", "", 7); snprintf(buf, sizeof(buf), "< %s", _lang("Back")); - create_label_item(cont, buf, 1, 7, 1); + create_label_item(cont, buf, 1, 8, 1); btn_group_set_sel(&btn_group_record_mode, g_setting.record.mode_manual ? 1 : 0); btn_group_set_sel(&btn_group_format, g_setting.record.format_ts ? 1 : 0); @@ -84,6 +110,7 @@ static lv_obj_t *page_record_create(lv_obj_t *parent, panel_arr_t *arr) { btn_group_set_sel(&btn_group_record_osd, g_setting.record.osd ? 0 : 1); btn_group_set_sel(&btn_group_record_audio, g_setting.record.audio ? 0 : 1); btn_group_set_sel(&btn_group_audio_source, g_setting.record.audio_source); + btn_group_set_sel(&btn_group_motor_audio, g_setting.record.motor_audio ? 0 : 1); btn_group_set_sel(&btn_group_file_naming, g_setting.record.naming); lv_obj_t *label2 = lv_label_create(cont); @@ -97,7 +124,7 @@ static lv_obj_t *page_record_create(lv_obj_t *parent, panel_arr_t *arr) { lv_obj_set_style_pad_top(label2, UI_PAGE_TEXT_PAD, 0); lv_label_set_long_mode(label2, LV_LABEL_LONG_WRAP); lv_obj_set_grid_cell(label2, LV_GRID_ALIGN_START, 1, 4, - LV_GRID_ALIGN_START, 8, 3); + LV_GRID_ALIGN_START, 9, 2); update_visibility(); @@ -134,7 +161,13 @@ static void page_record_on_click(uint8_t key, int sel) { btn_group_toggle_sel(&btn_group_audio_source); g_setting.record.audio_source = btn_group_get_sel(&btn_group_audio_source); ini_putl("record", "audio_source", g_setting.record.audio_source, SETTING_INI); + /* Re-apply line-out so DAC mix follows the new source selection. */ + dvr_select_audio_source(g_setting.record.audio_source); + dvr_enable_line_out(true); } else if (sel == 6) { + btn_group_toggle_sel(&btn_group_motor_audio); + apply_motor_audio_setting(btn_group_get_sel(&btn_group_motor_audio) == 0); + } else if (sel == 7) { if (rtc_has_battery() == 0) { btn_group_toggle_sel(&btn_group_file_naming); g_setting.record.naming = btn_group_get_sel(&btn_group_file_naming); @@ -146,7 +179,7 @@ static void page_record_on_click(uint8_t key, int sel) { page_pack_t pp_record = { .p_arr = { .cur = 0, - .max = 8, + .max = 9, }, .name = "Record Option", .create = page_record_create, diff --git a/src/util/time.c b/src/util/time.c index 219cec579..7d483b001 100644 --- a/src/util/time.c +++ b/src/util/time.c @@ -1,5 +1,6 @@ #include "time.h" +#include #include #include diff --git a/utilities/motor_audio_poc/CMakeLists.txt b/utilities/motor_audio_poc/CMakeLists.txt new file mode 100644 index 000000000..f048474bb --- /dev/null +++ b/utilities/motor_audio_poc/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.10) +project(motor_audio_poc C) + +set(CMAKE_C_STANDARD 99) +set(CMAKE_C_EXTENSIONS ON) +set(ROOT ${CMAKE_CURRENT_SOURCE_DIR}/../..) + +find_package(SDL2 REQUIRED) + +add_executable(motor_audio_poc + ${ROOT}/src/core/motor_audio.c + ${ROOT}/src/core/motor_rpm_proto.c + ${ROOT}/src/core/msp_displayport.c + ${ROOT}/src/util/time.c + ${ROOT}/src/emulator/motor_audio_sdl.c + ${ROOT}/src/emulator/motor_audio_dev.c + host_main.c + host_stubs.c +) + +target_include_directories(motor_audio_poc PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} + ${ROOT}/src + ${ROOT}/src/core + ${ROOT}/src/emulator + ${ROOT}/src/util + ${SDL2_INCLUDE_DIRS} +) + +# Prefer local host_log.h over lib/log via #include +file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/log) +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/host_log.h ${CMAKE_CURRENT_BINARY_DIR}/log/log.h COPYONLY) +target_include_directories(motor_audio_poc PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) + +target_compile_definitions(motor_audio_poc PRIVATE + EMULATOR_BUILD + HDZGOGGLE=1 + MOTOR_AUDIO_POC_HOST=1 + _GNU_SOURCE +) + +target_compile_options(motor_audio_poc PRIVATE -std=gnu99) + +target_link_libraries(motor_audio_poc PRIVATE + m + ${SDL2_LIBRARIES} +) + +if(APPLE) + # Prefer Homebrew SDL2 include path when CMake package lacks it + execute_process(COMMAND sdl2-config --cflags OUTPUT_VARIABLE SDL2_CFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE) + execute_process(COMMAND sdl2-config --libs OUTPUT_VARIABLE SDL2_LIBS OUTPUT_STRIP_TRAILING_WHITESPACE) + separate_arguments(SDL2_CFLAGS_LIST UNIX_COMMAND "${SDL2_CFLAGS}") + separate_arguments(SDL2_LIBS_LIST UNIX_COMMAND "${SDL2_LIBS}") + target_compile_options(motor_audio_poc PRIVATE ${SDL2_CFLAGS_LIST}) + target_link_libraries(motor_audio_poc PRIVATE ${SDL2_LIBS_LIST}) +endif() diff --git a/utilities/motor_audio_poc/README.md b/utilities/motor_audio_poc/README.md new file mode 100644 index 000000000..8d913de7c --- /dev/null +++ b/utilities/motor_audio_poc/README.md @@ -0,0 +1,5 @@ +# motor_audio_poc + +Standalone host build of the synthetic motor-audio PoC (protocol + PCM + blackbox replay). Use this on macOS/Linux when the full goggle emulator is impractical. + +See [docs/synthetic-motor-audio.md](../../docs/synthetic-motor-audio.md) for the wire format and CLI examples. diff --git a/utilities/motor_audio_poc/host_log.h b/utilities/motor_audio_poc/host_log.h new file mode 100644 index 000000000..760f35e7a --- /dev/null +++ b/utilities/motor_audio_poc/host_log.h @@ -0,0 +1,7 @@ +#pragma once + +/* Minimal stand-in for lib/log when building the host motor_audio PoC. */ +#define LOGI(...) ((void)0) +#define LOGW(...) ((void)0) +#define LOGE(...) ((void)0) +#define LOGD(...) ((void)0) diff --git a/utilities/motor_audio_poc/host_main.c b/utilities/motor_audio_poc/host_main.c new file mode 100644 index 000000000..2d35b337e --- /dev/null +++ b/utilities/motor_audio_poc/host_main.c @@ -0,0 +1,20 @@ +#include "emulator/motor_audio_dev.h" + +#include + +int main(int argc, char **argv) { + if (argc < 2) { + fprintf(stderr, + "motor_audio_poc host tool\n" + " --motor-test\n" + " --motor-synthetic [--motor-wav out.wav]\n" + " --motor-replay --motor-poles [--motor-wav out.wav]\n"); + return 1; + } + /* Always handled; exits internally on motor options. */ + if (!motor_audio_dev_main(argc, argv)) { + fprintf(stderr, "no motor-audio option recognized\n"); + return 1; + } + return 0; +} diff --git a/utilities/motor_audio_poc/host_stubs.c b/utilities/motor_audio_poc/host_stubs.c new file mode 100644 index 000000000..4ccb50207 --- /dev/null +++ b/utilities/motor_audio_poc/host_stubs.c @@ -0,0 +1,17 @@ +#include +#include + +/* Minimal stubs so the real msp_displayport.c can link on host. */ + +void load_fc_osd_font(uint8_t fhd) { + (void)fhd; +} + +void osd_signal_update(void) {} + +/* log stubs used if any LOG* path is compiled in */ +void log_print(int level, const char *tag, const char *fmt, ...) { + (void)level; + (void)tag; + (void)fmt; +} diff --git a/utilities/motor_audio_poc/osd.h b/utilities/motor_audio_poc/osd.h new file mode 100644 index 000000000..45898a33c --- /dev/null +++ b/utilities/motor_audio_poc/osd.h @@ -0,0 +1,4 @@ +#pragma once +#include +/* Host PoC shadow header: real src/core/osd.h is not required. */ +void load_fc_osd_font(uint8_t fhd);