Program Listing for File lander_gnc_app.h

↰ Return to documentation for file (applications/lander/lander_gnc_app.h)

#pragma once

#include <string>
#include <vector>

#include "lupnt/applications/application.h"
#include "lupnt/core/config.h"
#include "lupnt/core/definitions.h"

namespace lupnt {

  struct LanderGncConfig {
    std::string start_epoch_utc = "2027-03-01T00:00:00";
    double duration_s = 300.0;
    double dt_s = 0.5;

    double descent_start_east_m = -2500.0;
    double descent_start_north_m = 400.0;
    double descent_end_east_m = 0.0;
    double descent_end_north_m = 0.0;
    double descent_start_alt_m = 2000.0;
    double descent_end_alt_m = 15.0;
    double descent_heading_deg = 0.0;

    MatXd ref_traj_enu;
  };

  class LanderGncApp : public Application {
  public:
    LanderGncApp() = default;
    explicit LanderGncApp(Config& config);

    void SetReferenceTrajectoryEnu(const MatXd& ref_traj_enu) { cfg_.ref_traj_enu = ref_traj_enu; }

    void Setup() override;
    void Step(Real t) override;
    void Log(Real /*t*/) override {}

    void EnsureInitialized();

    // ---- Truth accessors (valid after EnsureInitialized) ----------------------
    int N() const { return N_; }
    double dt() const { return dt_; }
    const Vec3d& TruthPos(int k) const { return r_truth_[k]; }
    const Vec3d& TruthVel(int k) const { return v_truth_[k]; }
    const Mat3d& TruthAtt(int k) const { return R_truth_[k]; }
    const Vec3d& SpecificForce(int k) const { return f_body_truth_[k]; }
    const Vec3d& AngularRate(int k) const { return w_body_truth_[k]; }
    double Altitude(int k) const { return Alt_[k]; }
    Vec3d EnuTruth(int k) const { return Vec3d(Ee_[k], Nn_[k], Uu_[k]); }

    const LanderGncConfig& config() const { return cfg_; }
    const MatXd& traj_enu_truth() const { return traj_enu_truth_; }
    const VecXd& alt_truth() const { return alt_truth_res_; }

  private:
    void InitScenario();
    void WriteLanderState(int k);

    LanderGncConfig cfg_;
    bool initialized_ = false;
    int N_ = 0;
    double dt_ = 0.5;

    // Local ENU tangent frame (from the shared World).
    Mat3d R_enu2pa_ = Mat3d::Identity();
    Vec3d up_hat_pa_ = Vec3d::UnitZ();

    // Truth trajectory.
    std::vector<double> Ee_, Nn_, Uu_, Alt_;
    std::vector<Vec3d> r_truth_, v_truth_;
    std::vector<Mat3d> R_truth_;
    std::vector<Vec3d> f_body_truth_, w_body_truth_;

    MatXd traj_enu_truth_;
    VecXd alt_truth_res_;
  };

}  // namespace lupnt