FCC-Dynamics-C

Appearance

Body / World Tick — Multi-Rate Orchestration ​

> Status: NEW · 已对齐 PCR Master Blueprint v1.0 > 范畴: simulation/pipeline/{BodyTick, WorldTick, Scheduler, EventInterpreter}.{h,cpp} > 依赖: 全部业务子域（合法跨域聚合） > 被依赖: runtime/Runner

1. 问题陈述 ​

WorldTick 与 BodyTick 是仿真的热循环骨架。它们负责：

  1. 多速率调度：phys_dt（如 1ms）与 fcc_dt（如 20ms）的协调。
  2. 降维-演化-升维：probe → BodyRWS → 写回 WorldState。
  3. 并行单体演化：N 个 RocketBody 的 BodyTick 可独立并行。
  4. 事件路由：BodyTick 产出的 DiscreteEvent 在 World 末尾解释成 StageOp。
  5. 拓扑演化：调用 dynamics::algebra::evolve_topology 完成分级/分离。
  6. 总线路由：BusBuffer 的跨 body 消息通过 bus::IBus 串行投递。

本节给出完整 pipeline 与每个阶段的实现要点。

2. BodyTick：单体演化 pipeline ​

2.1 函数签名 ​

cpp
// simulation/pipeline/BodyTick.h
namespace sim {

// 输入：(BodyEnv, RocketBody) → 输出：(BodyOut, RocketBody', BodyLog)
BodyRWS<BodyOut> body_tick(Time dt);

struct BodyOut {
    contracts::DynOutFrame             dyn_out;       // 给 FCC 看的物理真值（已封装）
    std::vector<contracts::EngineEffect> engine_effects;
    std::vector<contracts::FinDeflection> fin_deflections;
    std::vector<contracts::DiscreteEvent> emitted_events;
};

} // namespace sim

2.2 完整 pipeline（伪代码） ​

cpp
BodyRWS<BodyOut> body_tick(Time dt) {
    return body_ask() >>= [dt](const BodyEnv& env) {
        return body_get() >>= [dt, &env](RocketBody body) -> BodyRWS<BodyOut> {

            BodyOut out;
            BodyLog log;

            // ─── ① Avionics 阶段（设备 FSM + IMU/GPS 量化 + ICU 计时） ───
            for (auto& eng : body.engines) {
                auto effect = avionics::device::ecu::step(
                    eng.ecu, eng.mech, body.bus, *eng.spec, env.aero, dt);
                out.engine_effects.push_back(effect);
            }
            for (auto& sv : body.servos) {
                avionics::device::scu::step(sv.scu, sv.mech, body.bus, *sv.spec, dt);
            }
            for (auto& fin : body.fins) {
                auto defl = avionics::device::fin_ctrl::step(
                    fin.ctrl, fin.mech, body.bus, *fin.spec, env.aero, dt);
                out.fin_deflections.push_back(defl);
            }
            for (auto& imu : body.imus) {
                avionics::device::imu::step(
                    imu.state, body.aux.spec_force_b, body.aux.omega_b,
                    body.bus, *imu.spec, env.current_time, dt);
            }
            for (auto& gps : body.gpss) {
                avionics::device::gps::step(
                    gps.state, body.spatial.pos_ecf, body.bus, *gps.spec, env.current_time, dt);
            }
            for (auto& icu : body.icus) {
                auto events = avionics::device::icu::step(
                    icu.state, body.bus, *icu.spec, env.current_time, dt);
                for (auto& e : events) out.emitted_events.push_back(e);
            }

            // ─── ② FCC tick（可选；多速率） ───
            // 由外层 WorldTick 注入 fcc_should_tick 标志；详见 §6
            if (body.fcc.has_value() && env.fcc_should_tick) {
                // 2.1 Bus → FccInFrame
                auto fcc_in = decode_bus_to_fcc_in(body.bus, env.current_time);
                // 2.2 调用 FCC（自己的 RWS）
                auto [fcc_out, fcc_new, fcc_log] =
                    body.fcc->tick(fcc_in, dt);
                body.fcc->state = fcc_new;
                // 2.3 FccOutFrame → Bus
                encode_fcc_out_to_bus(body.bus, fcc_out, env.current_time);
                log.fcc_log = fcc_log;
            }

            // ─── ③ Plant Physics 阶段（force computer，Forces Monoid 累加） ───
            // 注：这里调用的是 simulation/pipeline/factories/make_*_body_pipeline 编译好的
            //     CompiledDynamics.per_stage[body.world_stage]
            auto pipeline = compiled_dynamics.per_stage[body.world_stage];
            auto [forces, body_after_devices, force_log] = pipeline(out.engine_effects).run(env, body);
            body = body_after_devices;
            log += force_log;

            // ─── ④ Dynamics_core 积分阶段 ───
            auto [next_spatial, next_inertial, next_aux, integ_log] =
                dynamics::ode::integrate_rk4(env, body.spatial, body.inertial, forces, dt);
            body.spatial  = next_spatial;
            body.inertial = next_inertial;
            body.aux      = next_aux;
            log += integ_log;

            // ─── ⑤ 封装 DynOutFrame（给 FCC 看的真值） ───
            out.dyn_out = contracts::DynOutFrame {
                .body_id      = body.body_id,
                .pos_ecf      = env.frame.lic_to_ecf(body.spatial.pos_lic),
                .vel_ecf      = env.frame.lic_to_ecf_vel(body.spatial.vel_lic, body.spatial.pos_lic),
                .att_q        = body.spatial.q_lic_body,
                .omega_b      = body.aux.omega_b,
                .spec_force_b = body.aux.spec_force_b,
                .total_mass   = body.inertial.mass,
                .centroid_b   = body.inertial.centroid,
            };

            return body_put(body) >> body_tell(log) >> body_pure(out);
        };
    };
}

2.3 阶段顺序契约 ​

阶段输入输出副作用
① Avionics stepBus 上一周期命令 + body 物理真值EngineEffect / FinDefl / Events + 设备 FSM 推进写 body.bus
② FCC tickBus 上 IMU/GPS payloadFccOutFrame读写 body.bus + body.fcc.state
③ Plant PhysicsEngineEffect + body 物理真值 + BodyEnv.aero/traj/massForces（Monoid）无
④ Dynamics integrateForces + body.spatial/inertialnext spatial/inertial/aux无
⑤ DynOutFrame 封装body 演化后状态DynOutFrame无（纯打包）

强契约：

  • ①必须在③之前（EngineEffect 是③的输入）。
  • ②与①顺序：①先 publish 物理感测到 bus，②再 poll → 同 tick 内 FCC 看到本 tick 的 IMU/GPS。
  • ③④必须连续，中间不能插任何修改 RocketBody 的步骤。
  • ⑤纯打包，不修改 body。

> 反例：在④之后再做 avionics step。设备命令会延迟一个 tick，造成控制时序漂移。

3. WorldTick：全局编排 ​

3.1 函数签名 ​

cpp
// simulation/pipeline/WorldTick.h
namespace sim {

WorldRWS<WorldOut> world_tick(Time dt);

struct WorldOut {
    std::vector<contracts::DynOutFrame> per_body_out;   // 每个 body 一份
    std::vector<contracts::StageOp>     applied_ops;    // 本 tick 应用的拓扑算子
};

} // namespace sim

3.2 完整 pipeline（伪代码） ​

cpp
WorldRWS<WorldOut> world_tick(Time dt) {
    return world_ask() >>= [dt](const WorldEnv& we) {
        return world_get() >>= [dt, &we](WorldState ws) -> WorldRWS<WorldOut> {

            WorldOut wout;
            WorldLog wlog;

            // ─── A. 时间推进 ───
            ws.current_time = ws.current_time + dt;
            bool fcc_should_tick = scheduler.should_fcc_tick(ws.current_time);

            // ─── B. 并行单体演化 ───
            std::vector<BodyOut>            body_outs(ws.bodies.size());
            std::vector<contracts::DiscreteEvent> all_events;

            #pragma omp parallel for if (allow_parallel)
            for (size_t i = 0; i < ws.bodies.size(); ++i) {
                BodyEnv benv = probe::assemble_body_env(we, ws.bodies[i], ws.current_time);
                benv.fcc_should_tick = fcc_should_tick;

                auto [bout, body_new, blog] = body_tick(dt).run(benv, ws.bodies[i]);
                ws.bodies[i] = std::move(body_new);
                body_outs[i] = std::move(bout);
                wlog        += lift_body_log(blog);
            }

            // 串行收集
            for (size_t i = 0; i < body_outs.size(); ++i) {
                wout.per_body_out.push_back(body_outs[i].dyn_out);
                for (auto& e : body_outs[i].emitted_events) all_events.push_back(e);
            }

            // ─── C. 总线路由（跨 body 消息） ───
            // 把每个 RocketBody.bus 中 dst 不在本 body 的消息 publish 到全局 IBus
            // 同时从 IBus 拉外部消息进各 body.bus
            for (auto& body : ws.bodies) route_bus_local_to_global(body.bus, *external_bus);
            for (auto& body : ws.bodies) route_bus_global_to_local(*external_bus, body.bus);

            // ─── D. 事件解释 + 拓扑演化 ───
            for (const auto& event : all_events) {
                contracts::StageOp op = interpret_event(event, ws);
                if (std::holds_alternative<algebra::NoOp>(op)) continue;
                ws.bodies = dynamics::algebra::evolve_topology(ws.bodies, op);
                wout.applied_ops.push_back(op);
                wlog.applied_stage_ops.push_back(op);
            }

            return world_put(ws) >> world_tell(wlog) >> world_pure(wout);
        };
    };
}

3.3 阶段顺序契约 ​

阶段必要前提
A. 时间推进必须在 B 之前；BodyEnv.current_time 依赖此
B. 并行 BodyTick必须在 C/D 之前；C/D 依赖 BodyTick 的输出
C. 总线路由必须在 B 之后；BusBuffer 在 BodyTick 中写入
D. 拓扑演化必须在 B 之后；事件来源于 BodyTick
C 与 D 同顺序：先 C 后 D设计选择（D 可能产生新 body，新 body 的初始 bus 应该是干净的）

4. 升维：BodyLog → WorldLog ​

lift_body_log(BodyLog) → WorldLog 把单体日志按类别合并进全局日志：

cpp
WorldLog lift_body_log(const BodyLog& bl) {
    return WorldLog {
        .force_traces      = bl.force_traces,        // 直接 copy
        .bus_traces        = bl.bus_traces,
        .fcc_traces        = bl.fcc_traces,
        .emitted_events    = bl.emitted_events,
        .applied_stage_ops = {},                     // World 层自己填
    };
}

并合通过 WorldLog::operator+=（Monoid）完成：

cpp
WorldLog& operator+=(WorldLog& a, const WorldLog& b) {
    a.force_traces.insert(a.force_traces.end(), b.force_traces.begin(), b.force_traces.end());
    a.bus_traces.insert(...);
    // ... 字段累加 ...
    return a;
}

Monoid 律：结合律（(a + b) + c == a + (b + c)）+ 单位元（空 log）。详见 05_Dynamics_Core/Forces_Monoid.md §3。

5. 事件解释器：DiscreteEvent → StageOp ​

cpp
// simulation/pipeline/EventInterpreter.h
namespace sim {

contracts::StageOp interpret_event(
    const contracts::DiscreteEvent& event,
    const WorldState& ws);

} // namespace sim

职责：把"硬件事件"翻译成"拓扑操作"。

cpp
contracts::StageOp interpret_event(const DiscreteEvent& e, const WorldState& ws) {
    using Kind = contracts::DiscreteEvent::Kind;

    switch (e.kind) {
        case Kind::EngineShutdown:
            // 单纯关机不改拓扑
            return algebra::NoOp{};

        case Kind::BoltFired:
            // 火工品触发 → 后续 StageSeparation 事件会来
            return algebra::NoOp{};

        case Kind::StageSeparation:
            // 分级：从 source body 析出 new body
            return algebra::SeparationOp {
                .source             = e.source_body,
                .new_body           = derive_new_body_id(e),
                .detached_engines   = ws.get_engines_to_detach(e.source_body, e.payload_id),
                .avionics_for_new   = ws.get_avionics_action(e.source_body, e.payload_id),
            };

        case Kind::FinDeploy:
            // 栅格舵展开 → world_stage 转换
            return algebra::TransitionOp {
                .body = e.source_body,
                .next = algebra::WorldStage::REENTRY,
            };
    }
    return algebra::NoOp{};
}

关键归属：

  • 事件来源：ICU FSM（avionics 子域）
  • 事件接收：DynInFrame.events（contracts/）
  • 事件解释：sim::interpret_event（simulation/，唯一允许跨域聚合层）
  • 事件应用：dynamics::algebra::evolve_topology（dynamics_core/，普适代数）

> 这是 Blueprint §2.9 数据流的完整闭环。 > simulation/ 决定一个 BoltFired 事件是变成 SeparationOp 还是 NoOp——这是策略，不是普适物理。

详见 03_Avionics_and_Bus/Sensor_Modeling.md（ICU FSM）和 05_Dynamics_Core/Topology_Algebra.md（StageOp 代数）。

6. 多速率调度 ​

6.1 Scheduler ​

cpp
// simulation/pipeline/Scheduler.h
namespace sim {

struct MultiRateScheduler {
    Time fcc_period   { Time::from_hz(50) };       // FCC 默认 50Hz
    Time phys_period  { Time::from_hz(1000) };     // 物理默认 1kHz
    Time last_fcc_tick { Time::zero() };

    bool should_fcc_tick(Time now) {
        if (now - last_fcc_tick >= fcc_period) {
            last_fcc_tick = now;
            return true;
        }
        return false;
    }
};

} // namespace sim

6.2 时序示意 ​

phys tick (1ms):    │1│2│3│4│5│6│7│8│9│...│20│21│...│40│41│...
                    │ │ │ │ │ │ │ │ │ │   │  │  │   │  │
FCC tick (50Hz):    ★                     ★              ★    ← 每 20 个 phys tick 一次
                    │                     │              │
fcc_should_tick:    T F F F F F F F F F...T  F  F...F   T  F...

关键：

  • phys_dt 一定整除 fcc_dt（否则会出现"跳过 FCC tick"或"漂移"）。
  • FCC 频率由 YAML 配置（run_xxx.yaml: scheduler.fcc_hz: 50），不是硬编码。
  • HIL 模式下 FCC 在另一台机器，scheduler 只控制 plant 端何时把数据打包发出。

6.3 频率匹配示例 ​

用例phys_hzfcc_hzphys_dtfcc_dt每多少 phys 一次 FCC
常规 SIL1000501ms20ms20
高频 HIL10001001ms10ms10
高保真 phys400050250μs20ms80
低频 sim 调试1002510ms40ms4

6.4 反例 ​

  • fcc_hz = 33, phys_hz = 1000 → fcc_dt = 30.30..ms，不整除 → 漂移
  • 每个 phys tick 都 should_fcc_tick 返回 true → FCC 50Hz 控制环退化成 1kHz → 增益爆炸

7. 并行化 ​

7.1 默认串行 ​

WorldTick 默认串行调用 BodyTick（简洁性 + 易调试）。

7.2 启用并行 ​

cpp
// 配置
struct ScheduleConfig {
    bool allow_parallel_body_tick = false;
    int  body_tick_thread_count   = 0;   // 0 = auto
};

启用条件（详见 Dual_Layer_RWS.md §8）：

  • WorldEnv 装配后只读。
  • 每个 BodyEnv 是独立栈对象。
  • BodyTick 不修改 WorldState（事件、日志通过 BodyOut/BodyLog 出栈）。
  • 总线路由放在 BodyTick 之外（C 阶段串行）。

7.3 内部实现 ​

OpenMP / TBB / 手工线程池都可以。推荐 OpenMP（compile-time switch）：

cpp
#pragma omp parallel for if (config.allow_parallel_body_tick) num_threads(N)
for (size_t i = 0; i < ws.bodies.size(); ++i) {
    // ... BodyTick ...
}

> 不要 在 BodyTick 内部再起线程（嵌套并行造成 oversubscription）。一层并行足够。

8. 完整 tick 流程图 ​

runtime::Runner::run_loop():
    while (!finished):
        world_tick(phys_dt).run(world_env, world_state)
            │
            ▼
        ┌──── WorldTick (simulation/) ────────────────────┐
        │ A. 时间推进                                       │
        │      ws.current_time += dt                       │
        │      fcc_should_tick = scheduler.should_*(t)     │
        │                                                  │
        │ B. for each body_i (可并行):                     │
        │      ┌── probe::assemble_body_env ──────────┐    │
        │      │  TrajCtx + AeroCtx + MassPropsCtx    │    │
        │      │  + asset_ptr + atmosphere_ptr + ...  │    │
        │      └────────────┬─────────────────────────┘    │
        │                   ▼                              │
        │      ┌──── BodyTick (simulation/) ─────────┐     │
        │      │ ① Avionics step (设备 FSM × N)      │     │
        │      │ ② FCC tick (若 should_tick)          │     │
        │      │ ③ Plant Physics (Forces Monoid)      │     │
        │      │ ④ Dynamics integrate (RK4)          │     │
        │      │ ⑤ DynOutFrame 封装                   │     │
        │      └────────────┬─────────────────────────┘    │
        │                   ▼                              │
        │      ws.bodies[i] = body_new                     │
        │      world_log   += lift(body_log)               │
        │                                                  │
        │ C. 总线路由 (串行)                                │
        │      local BusBuffer ←→ global IBus              │
        │                                                  │
        │ D. 事件解释 + 拓扑演化                            │
        │      for event in all_events:                    │
        │        op = interpret_event(event, ws)           │
        │        ws.bodies = evolve_topology(ws.bodies, op)│
        │                                                  │
        │      return (WorldOut, ws_new, world_log)        │
        └──────────────────────────────────────────────────┘

9. 不变量与契约 ​

契约强度检查点
BodyTick 内部不修改 WorldState必须并行合法性根本依赖
BodyTick 阶段顺序：avionics → fcc → physics → integrate必须时序契约
probe 在 BodyTick 入口调用一次必须各算子时间一致性
WorldTick 顺序：A → B → C → D必须拓扑事件依赖 BodyTick 输出
拓扑演化只在 World 末尾发生必须per-stage 预编译派发的前提
FCC tick 频率 整除 phys 频率必须调度无漂移
总线路由发生在 BodyTick 之后必须跨 body 消息一致性
body.fcc 可为空（分离的子箭）设计BodyTick.②跳过即可
BodyOut.emitted_events 收集而非直接修改 ws必须并行 + 事件顺序确定性

10. 与三种部署模式的适配 ​

部署BodyTickWorldTick
avionics_dry①②跑，③④跳过（物理冻结）A B（仅 avionics）C，D 通常 NoOp
sil_monolithic①②③④⑤ 全跑A B C D 全跑
hil_dyn①跑，②的 FCC 调用换成 DynChannel send/recvA B C D 全跑
hil_fccplant 端跑 ①③④⑤；FCC 在 RTOSplant 端 A B C D，FCC 端独立

> HIL 的本质是信道替换：DynChannel 替换 FCC↔Plant 数据流，IBus 替换 FCC↔Device 数据流。BodyTick / WorldTick 骨架不变（Blueprint §1.3）。

详见 07_Runtime/Assembler.md 与 07_Runtime/IDynChannel.md（待写）。

11. 反模式 ​

反模式后果正确做法
BodyTick 中段调用 probe_aero（"我换了高度，重算一次"）不同算子看到不同环境；时间不一致probe 在入口 once，整 tick 共享
BodyTick 直接修改 world_state.bodies[i]越层 + 并行不安全通过 BodyRWS State 操作 body 局部
BodyTick 内部直接调用 algebra::evolve_topology拓扑演化要求 ws.bodies 全集出栈到 BodyOut.emitted_events，World 末尾统一
FCC tick 在 ①之前调用FCC 看不到本 tick 的传感量化结果② 必须在 ① 之后
跨 body 直接共享 Bus 句柄与 BusBuffer 设计矛盾；并行竞争BusBuffer 在 RocketBody 内；跨 body 走 IBus 串行
WorldTick 中重新装配 WorldEnv严重违背"装配冻结"契约；指针失效WorldEnv 在 Runner 构造时装配 once
把 scheduler 放到 BodyTick 内各 body 调度不一致；FCC 频率不可控scheduler 在 WorldTick (A 阶段) 决定
在并行 for 中读写共享 vector数据竞争每个 thread 写 body_outs[i]（独立下标），事件最后串行收集
直接 compute_thrust(world_env, body)算子跨域；签名爆炸算子签名 (BodyEnv, RocketBody)，由 probe 降维
BodyTick 内直接 ws.event_history.push_back数据竞争写 BodyLog.emitted_events，World 升维

12. C-Distillation 路径 ​

C++ 抽象C 蜕化
WorldRWS<WorldOut> / BodyRWS<BodyOut> 模板world_tick(WorldEnv*, WorldState*, WorldLog*, WorldOut*, dt) C 函数
body_tick(dt).run(env, body)body_tick_c(const BodyEnv*, RocketBody*, BodyOut*, BodyLog*, dt)
for_each_body 并行RTOS 任务表或裸核 SMP
std::vector<DiscreteEvent>编译期上限静态数组 + count
interpret_event（变体匹配）switch case + StageOp tagged union
MultiRateScheduler计数器 + 整数比较
lift_body_logmemcpy + count 累加
OpenMP #pragma omp parallel for任务表派发

详见 09_Cross_Cutting/C_Distillation.md。

13. 测试策略 ​

13.1 单元层 ​

  • interpret_event(DiscreteEvent) 各 Kind 分支返回正确 StageOp。
  • Scheduler::should_fcc_tick 在 N 个 phys_dt 后恰好返回 1 次 true。

13.2 组件层 ​

  • BodyTick 单步：固定 BodyEnv + 固定 RocketBody，跑 1 步，验证 5 阶段输出。
  • BodyTick 阶段顺序：用桩验证 compute_thrust 在 ecu::step 之后调用。
  • 多 body BodyTick 并行 = 串行：单测验证给定相同输入，并行/串行结果 bit-identical。

13.3 集成层 ​

  • 1s SIL：验证 1000 tick 后 spatial 在期望误差内。
  • 阶段分离：mock ICU 在 t=10s 触发 StageSeparation → 验证 ws.bodies.size() 从 1 变 2。
  • 多速率：phys 1kHz + FCC 50Hz，1s 内 FCC tick 计数 = 50。

详见 08_Verification/Test_Strategy.md。

14. 引用 ​

  • Blueprint §1.2（完整 tick 流程图）、§2.6.4（双层 RWS 实例化与多速率）、§2.9（事件 → StageOp 数据流）、§3.2（FCC ↔ simulation 装组拆卸）
  • Dual_Layer_RWS.md（RWS 三元组与升维降维）
  • RocketBody_Composite.md（BodyTick 操作的 State）
  • WorldEnv_Assembly.md（probe 函数实现）
  • 04_FCC/Pipeline_Factory_and_Compilation.md（fcc.tick 内部为 per-stage 预编译派发）
  • 05_Dynamics_Core/Topology_Algebra.md（StageOp 与 evolve_topology）
  • 05_Dynamics_Core/Forces_Monoid.md（force computer 累加）
  • 03_Avionics_and_Bus/Semantic_Bus_Pattern.md（BusBuffer ↔ IBus 路由）
  • 03_Avionics_and_Bus/Sensor_Modeling.md（设备 step 函数实现）