重力补偿模块考虑空气阻力

extra/compensator/CMakeLists.txt

@@ -1,5 +1,5 @@
 rmvl_add_module(
-  compensator
+  compensator INTERFACE
   DEPENDS group
 )
 
@@ -26,3 +26,12 @@ rmvl_add_module(
 )
 
 rmvl_generate_module_para(compensator)
+
+# Tests
+if(BUILD_TESTS)
+  rmvl_add_test(
+    compensator Unit
+    DEPENDS gravity_compensator
+    DEPEND_TESTS GTest::gtest_main
+  )
+endif()

extra/compensator/include/rmvl/compensator/compensator.h

@@ -63,34 +63,7 @@ class compensator
      * @param[in] com_flag 手动调节补偿标志
      * @return 补偿模块信息
      */
-    virtual CompensateInfo compensate(const std::vector<group::ptr> &groups, uint8_t shoot_speed,
-                                      CompensateType com_flag) = 0;
-
-    /**
-     * @brief 获得补偿角度
-     * @note
-     * - 使用不动点迭代法进行补偿量的求解 \cite icra2019
-     * - 需要严格满足相机对水平方向的夹角等于 `gyro_angle.y`
-     *
-     * @param[in] x 目标离相机的水平宽度
-     * @param[in] y 目标离相机的铅垂高度
-     * @param[in] velocity 枪口射速
-     *
-     * @return 补偿角度
-     */
-    static double getPitch(double x, double y, double velocity);
-
-private:
-    /**
-     * @brief 计算真实的 y 坐标
-     *
-     * @param[in] distance 相机到装甲板的水平距离 (m)
-     * @param[in] velocity 枪口射速 (m/s)
-     * @param[in] angle 枪口与水平方向的夹角 (rad)
-     *
-     * @return 世界坐标系下真实的 y 坐标
-     */
-    static double bulletModel(double distance, double velocity, double angle);
+    virtual CompensateInfo compensate(const std::vector<group::ptr> &groups, float shoot_speed, CompensateType com_flag) = 0;
 };
 
 //! @} compensator

extra/compensator/include/rmvl/compensator/gravity_compensator.h

@@ -9,6 +9,8 @@
  *
  */
 
+#pragma once
+
 #include "compensator.h"
 
 namespace rm
@@ -21,32 +23,25 @@ namespace rm
 class GravityCompensator final : public compensator
 {
 public:
-    GravityCompensator();
+    GravityCompensator() noexcept;
+    ~GravityCompensator();
 
     //! 构造 GravityCompensator
-    static inline std::unique_ptr<GravityCompensator> make_compensator()
-    {
-        return std::make_unique<GravityCompensator>();
-    }
+    static inline auto make_compensator() { return std::make_unique<GravityCompensator>(); }
 
     /**
-     * @brief 补偿核心函数
+     * @brief 补偿函数，考虑空气阻力，使用 2 阶龙格库塔方法（中点公式）计算弹道
      *
      * @param[in] groups 所有序列组
      * @param[in] shoot_speed 子弹射速 (m/s)
      * @param[in] com_flag 手动调节补偿标志
      * @return 补偿模块信息
      */
-    CompensateInfo compensate(const std::vector<group::ptr> &groups,
-                              uint8_t shoot_speed, CompensateType com_flag) override;
+    CompensateInfo compensate(const std::vector<group::ptr> &groups, float shoot_speed, CompensateType com_flag) override;
 
 private:
-    /**
-     * @brief 更新静态补偿量
-     *
-     * @param[in] com_flag 补偿类型
-     */
-    void updateStaticCom(CompensateType com_flag);
+    class Impl;
+    Impl *_impl;
 };
 
 //! @} compensator

extra/compensator/include/rmvl/compensator/gyro_compensator.h

@@ -9,6 +9,8 @@
  *
  */
 
+#pragma once
+
 #include "compensator.h"
 
 namespace rm
@@ -21,32 +23,21 @@ namespace rm
 class GyroCompensator final : public compensator
 {
 public:
+    //! 创建 GyroCompensator 对象
     GyroCompensator();
 
-    //! 构造 GyroCompensator
-    static inline std::unique_ptr<GyroCompensator> make_compensator()
-    {
-        return std::make_unique<GyroCompensator>();
-    }
+    //! 使用静态工厂函数创建 GyroCompensator 对象
+    static inline auto make_compensator() { return std::make_unique<GyroCompensator>(); }
 
     /**
-     * @brief 补偿核心函数
+     * @brief 补偿函数，未考虑空气阻力，仅使用抛物线模型 \cite icra2019
      *
      * @param[in] groups 所有序列组
      * @param[in] shoot_speed 子弹射速 (m/s)
      * @param[in] com_flag 手动调节补偿标志
      * @return 补偿模块信息
      */
-    CompensateInfo compensate(const std::vector<group::ptr> &groups,
-                              uint8_t shoot_speed, CompensateType com_flag) override;
-
-private:
-    /**
-     * @brief 更新静态补偿量
-     *
-     * @param[in] com_flag 补偿类型
-     */
-    void updateStaticCom(CompensateType com_flag);
+    CompensateInfo compensate(const std::vector<group::ptr> &groups, float shoot_speed, CompensateType com_flag) override;
 };
 
 //! @} compensator

extra/compensator/param/gravity_compensator.para

@@ -1,3 +1,15 @@
-float YAW_COMPENSATE = 0.f   # yaw 静态补偿 (相机比测速模块高出的角度)
-float PITCH_COMPENSATE = 0.f # pitch 静态补偿 (相机比测速模块高出的角度)
-float MINIMUM_COM = 0.5f     # 手动补偿最小步进
+#################### 物理参数 ####################
+double g = 9.788     # 重力加速度，单位 m/s^2
+double m = 0.041     # 质量，单位 kg
+double rho = 1.29    # 空气密度，单位 kg/m^3
+double A = 1.4186e-3 # 参考面积，单位 m^2
+double V = 4.0194e-5  # 体积，单位 m^3
+double Cd = 0.26     # 阻力系数
+double Cl = 0.2      # 升力系数
+
+################## 补偿调节参数 ##################
+double h = 0.02            # 龙格库塔迭代步长
+size_t MAX_COM = 100       # 最大补偿步进
+float YAW_COMPENSATE = 0   # yaw 静态补偿 (相机比测速模块高出的角度)
+float PITCH_COMPENSATE = 0 # pitch 静态补偿 (相机比测速模块高出的角度)
+float MINIMUM_COM = 0.5    # 手动补偿最小步进

extra/compensator/param/gyro_compensator.para

@@ -1,3 +1,7 @@
+#################### 物理参数 ####################
+double g = 9.788   # 重力加速度，单位 m/s^2
+
+################## 补偿调节参数 ##################
 float YAW_COMPENSATE = 0.f   # yaw 静态补偿 (相机比测速模块高出的角度)
 float PITCH_COMPENSATE = 0.f # pitch 静态补偿 (相机比测速模块高出的角度)
 float MINIMUM_COM = 0.5f     # 手动补偿最小步进