FCC-Dynamics-C

Appearance

Algorithm Integration Guide for FCC ​

> Status: PATCH · 已对齐 PCR Master Blueprint v1.0 > 范畴: fcc/algorithms/{nav,guidance,control}/ > 依赖: fcc/state/FccState.h、contracts/、types/

1. 你的工作位置（v1.0） ​

GNC 算法开发者：你不再需要碰 DSL、解释器或 RWS。你只在一个地方写代码：

src/fcc/algorithms/
├── nav/                    # 导航（你的导航算法）
│   ├── INavigator.h        # 接口
│   ├── DefaultNavigator.{h,cpp}
│   ├── KalmanNavigator.{h,cpp}
│   └── ...
├── guidance/               # 制导
│   ├── IGuidance.h
│   ├── PitchProgramGuidance.{h,cpp}
│   ├── GravityTurnGuidance.{h,cpp}
│   └── ...
└── control/                # 控制
    ├── IController.h
    ├── PidController.{h,cpp}
    ├── LqrController.{h,cpp}
    └── ...

关键变化（v0 → v1）：

  • ❌ 不要改 fcc/interpreter/FccInterpreter.cpp（这是基础设施，与算法无关）
  • ❌ 不要改 fcc/dsl/FccProgram.h（这是算子描述，与算法无关）
  • ✓ 只在 fcc/algorithms/<nav|guidance|control>/ 添加你的纯函数实现

2. 接口契约 ​

每个 GNC 模块实现一个接口，输入是 const FccState& 与若干测量值，输出是新状态字段。纯函数，无副作用：

2.1 Navigation ​

cpp
// fcc/algorithms/nav/INavigator.h
namespace fcc::nav {

struct NavInput {
    Time         current_time;
    Imu          last_imu;        // d_vel, d_theta, valid
    GpsFix       last_gps;        // optional
    NavState     prev;            // 上一帧 nav 状态
};

struct NavOutput {
    Vec3   pos_est_lic;
    Vec3   vel_est_lic;
    Quat   att_est_lic_body;
    Vec3   bias_gyro;
    Vec3   bias_acc;
    bool   nav_valid;
};

class INavigator {
public:
    virtual ~INavigator() = default;
    virtual NavOutput step(const NavInput& in) = 0;
};

} // namespace fcc::nav

2.2 Guidance ​

cpp
// fcc/algorithms/guidance/IGuidance.h
namespace fcc::guidance {

struct GuidanceInput {
    Time         current_time;
    NavState     nav;             // 当前 nav 估计
    FccStage     stage;           // 当前 FCC 状态机阶段
    GuidanceState prev;
};

struct GuidanceOutput {
    Vec3        target_pos_lic;
    Vec3        target_vel_lic;
    Quat        target_att;
    double      pitch_cmd;        // for scripted profile
};

class IGuidance {
public:
    virtual ~IGuidance() = default;
    virtual GuidanceOutput step(const GuidanceInput& in) = 0;
};

} // namespace fcc::guidance

2.3 Control ​

cpp
// fcc/algorithms/control/IController.h
namespace fcc::control {

struct ControlInput {
    Time             current_time;
    NavOutput        nav;
    GuidanceOutput   guide;
    ControlState     prev;        // PID 积分器、滤波器状态
};

struct ControlOutput {
    double  thrust_norm;          // 0..1
    Vec3    torque_cmd;           // Nm in body frame
    // 或：直接的 TVC 角、栅格舵角
    Vec3    integral_error_new;
    Vec3    last_error_new;
};

class IController {
public:
    virtual ~IController() = default;
    virtual ControlOutput step(const ControlInput& in) = 0;
};

} // namespace fcc::control

> 统一签名：step(In) → Out，纯函数，无 RWS，无 Bus，无 IBus/IDynChannel。 > 这是 Blueprint §3.1 "FCC 三层架构"中最底的纯算法层。

3. 怎么写一个新算法 ​

3.1 例子：写一个简单 PID 高度控制器 ​

cpp
// fcc/algorithms/control/AltitudePidController.h
#pragma once
#include "fcc/algorithms/control/IController.h"

namespace fcc::control {

struct AltitudePidGains {
    double kp = 0.5;
    double ki = 0.1;
    double kd = 0.05;
};

class AltitudePidController : public IController {
public:
    explicit AltitudePidController(AltitudePidGains g) : g_(g) {}

    ControlOutput step(const ControlInput& in) override {
        double z      = in.nav.pos_est_lic.z();
        double z_cmd  = in.guide.target_pos_lic.z();
        double err    = z_cmd - z;

        // 积分（带反饱和）
        Vec3 i_new = in.prev.integral_error;
        i_new.z() = std::clamp(in.prev.integral_error.z() + g_.ki * err * dt(in), -1.0, 1.0);

        // 微分
        Vec3 d_new;
        d_new.z() = (err - in.prev.last_error.z()) / dt(in);

        // 输出
        double thrust = g_.kp * err + i_new.z() + g_.kd * d_new.z();
        thrust = std::clamp(thrust, 0.0, 1.0);

        return ControlOutput{
            .thrust_norm        = thrust,
            .torque_cmd         = Vec3::zero(),
            .integral_error_new = i_new,
            .last_error_new     = Vec3{0, 0, err},
        };
    }

private:
    AltitudePidGains g_;
    static double dt(const ControlInput& in) { return /* TODO from time diff */ 0.02; }
};

} // namespace fcc::control

3.2 接入 FCC：mission 配置 + DSL 编译 ​

你的算法不会自己进入 FCC——它通过 mission profile（fcc/firmware/MissionProfile.{h,cpp}）声明，被 runtime::Assembler 在装配期 instantiate 并组装到 FccEnv.gnc_modules：

cpp
// fcc/firmware/MissionProfile.cpp
fcc::FccEnv build_fcc_env(const FccConfig& cfg) {
    fcc::FccEnv env;
    env.nav        = std::make_unique<fcc::nav::KalmanNavigator>(cfg.nav_params);
    env.guidance   = std::make_unique<fcc::guidance::PitchProgramGuidance>(cfg.guide_params);
    env.controller = std::make_unique<fcc::control::AltitudePidController>(cfg.pid_gains);
    // ...
    return env;
}

mission YAML：

yaml
# data/input/fcc_v1/mission.yaml
gnc:
  navigator:   KalmanNavigator
  guidance:    PitchProgramGuidance
  controller:  AltitudePidController
  pid_gains:
    kp: 0.5
    ki: 0.1
    kd: 0.05

3.3 FccInterpreter 怎么调你 ​

解释器把 Free Monad tree 翻译成 RWS 动作。当它解释到 RunNavigation 算子时（详见 04_FCC/Interpreter_and_RWS.md），调用：

cpp
// fcc/interpreter/FccInterpreter.cpp（基础设施代码，你不动）
RWS<FccEnv, FccState, FccLog, std::monostate>
operator()(const RunNavigation& op) {
    return ask() >>= [](const FccEnv& env) {
        return get_state() >>= [&env](FccState s) {
            // 调用你的 INavigator 实现
            auto nav_out = env.nav->step({
                .current_time = s.clock.current_time,
                .last_imu     = s.last_imu,
                .last_gps     = s.last_gps,
                .prev         = s.navigation,
            });
            s.navigation.pos_est_lic = nav_out.pos_est_lic;
            s.navigation.vel_est_lic = nav_out.vel_est_lic;
            // ...
            return put_state(s);
        };
    };
}

关键：解释器持有 FccEnv.nav/guidance/controller 接口指针；你的算法是接口的具体实现。你写实现，解释器写 dispatch。

4. 算法的状态在哪？ ​

你的状态存储位置
上一帧的传感量化输出FccState.last_imu, FccState.last_gps
你的 nav 输出FccState.navigation（NavState 子结构）
你的 guide 输出FccState.guidance（GuidanceState 子结构）
你的 control 内部（PID 积分器）FccState.control（ControlState 子结构）
不要 把状态藏在你的对象里（如 KalmanNavigator::P_）因为 FCC 可能 reset、回滚、多实例

> 强契约：算法对象（INavigator/IGuidance/IController 实例）只存增益/参数，不存运行时状态。运行时状态全部进 FccState，每帧由解释器读入算法 + 写回 FccState。

> 理由：FCC 状态可序列化、可热替换、可重置——这是 Blueprint §3.1 / §3.4 的硬性要求。

5. FCC 不能看到的东西 ​

数据你访问不到（设计如此）
物理真值 RocketBody.spatial/inertial/aux这是 plant 层；FCC 只通过 IMU/GPS 量化值看到一部分
大气/重力大表这是 environment 层；FCC 内部 nav 用 FccEnv.gravity_model_nominal 等简化模型
其他 body 的状态FCC 是 per-body 的
WorldState.bodiesFCC 不在 simulation 层
bus::IBus 直接访问FCC 通过 ReadIMU / OutputControls 等算子，DSL 隐藏 bus

> 理由：FCC 必须能 distill 到 C，必须能跑 RTOS。访问大表 / WorldState 会让 binary 暴涨且无法移植。

6. 频率与 FccStage ​

你的算法什么时候被调用，由两个东西决定：

  1. MultiRateScheduler（simulation/pipeline/Scheduler.h）：决定 FCC 整体 tick 频率（默认 50Hz）。这是 simulation 层的事，你不管。
  2. FccStage（04_FCC/FCC_State_Machine.md）：决定当前阶段编译进 FCC 的算子树是否包含 RunNavigation/RunGuidance/RunControl。

例如：

  • PRE_LAUNCH 阶段：可能只跑 nav，不跑 guide/control
  • ASCENT 阶段：跑 nav + guide + control（全开）
  • COAST 阶段：跑 nav，guide/control 进 hold 模式

mission 工程师在 fcc/firmware/MissionProfile 中为每个 FccStage 预编译一棵算子树（详见 04_FCC/Pipeline_Factory_and_Compilation.md）。运行期由 compiled_fcc.per_stage[fcc_state.stage] O(1) 派发。

> 作为算法开发者：你的 step(In) → Out 函数 不要自己检查 stage。stage 切换由 mission profile 决定要不要调用你。

7. 单元测试你的算法 ​

因为接口纯函数，单测简单：

cpp
TEST(AltitudePidControllerTest, ProportionalCorrection) {
    fcc::control::AltitudePidGains g{ .kp = 0.5, .ki = 0.0, .kd = 0.0 };
    fcc::control::AltitudePidController ctrl(g);

    fcc::control::ControlInput in;
    in.nav.pos_est_lic   = Vec3{0, 0, 1000};
    in.guide.target_pos_lic = Vec3{0, 0, 1100};
    in.prev = {};   // 零初始

    auto out = ctrl.step(in);
    EXPECT_NEAR(out.thrust_norm, 0.5, 1e-6);   // 0.5 * (1100 - 1000) / 100 = 0.5
}

不需要 mock RWS、不需要 mock Bus、不需要 mock simulation——你的代码完全可测。

更多见 08_Cross_Cutting/Testing_Framework.md。

8. 反模式 ​

反模式后果正确做法
在 FccInterpreter.cpp 里写算法算法 + dispatch 耦合；多算法切换困难；与解释器演化纠缠算法进 fcc/algorithms/*/<NameAlgo>.{h,cpp}，实现 INavigator/IGuidance/IController
算法对象内部藏运行时状态（KalmanNavigator::P_）不可序列化；reset 失败；多 body 共享时数据竞争状态全进 FccState，每帧读入算法 + 写回 FccState
算法直接读 RocketBody.spatial.pos_lic越层，FCC 不该看到物理真值只读 FccState.last_imu / last_gps
算法直接 bus.publish跨层；c-distill 失败用 DSL 算子 OutputControls(...)，解释器负责进 bus
算法 if (stage == LAUNCH) ...stage 派发与算法耦合mission profile 为每个 stage 编译不同算子树
算法用 std::function<...> 持有 lambda 配置C-distillation 受阻用 POD 增益结构 + 显式分支
算法引入 heap 分配RTOS 不友好编译期固定大小 buffer；运行期 zero-allocation
算法内 std::cout << "debug"RWS 纯性破坏用 WriteTelemetry(...) 算子，进 FccLog

9. C-Distillation 路径 ​

C++C
INavigator 虚类函数指针：int (*nav_step)(const NavInput*, NavOutput*)
std::unique_ptr<INavigator>静态函数指针 + 静态状态
std::vector<double> 增益编译期 static const double gains[N]
std::clamp普通 C 函数
Vec3 / Quat 类C struct + 自由函数

算法层是 FCC 中最重要的 distillation target——必须保持极简、POD-only、无堆分配。

详见 09_Cross_Cutting/C_Distillation.md（待写）与 Blueprint §7.12。

10. 引用 ​

  • Blueprint §3.1（FCC 三层架构）、§3.4（FccState GNC Mealy 机）、§7.12（C-Distillation 软化）
  • 04_FCC/Free_Monad_DSL.md（算子描述层）
  • 04_FCC/Interpreter_and_RWS.md（解释器如何调用你的算法）
  • 04_FCC/Pipeline_Factory_and_Compilation.md（mission profile 编译）
  • 04_FCC/FCC_State_Machine.md（FccStage 状态机）
  • 04_FCC/Static_Compilation_FSM.md（per-stage 预编译细节）
  • 08_Cross_Cutting/Symmetric_RWS_Philosophy.md（算法 vs 解释器分层裁定）
  • 08_Cross_Cutting/Testing_Framework.md（如何单测）