Thrusters control v1.0

hydrv_thruster/CMakeLists.txt

@@ -7,12 +7,18 @@ set(CMAKE_CXX_STANDARD_REQUIRED True)
 
 if(MCU_FAMILY STREQUAL "F4")
     add_library(${LIBRARY_NAME} INTERFACE)
-    target_link_libraries(${LIBRARY_NAME} INTERFACE HydrolibCommon)
-    target_link_libraries(${LIBRARY_NAME} INTERFACE HydrodriversGPIO)
-    target_link_libraries(${LIBRARY_NAME} INTERFACE HydrodriversTimerLow)
-    target_link_libraries(${LIBRARY_NAME} INTERFACE CMSIS)
+    target_link_libraries(${LIBRARY_NAME}
+        INTERFACE
+        HydrolibCommon
+        HydrodriversGPIO
+        HydrodriversTimerLow
+        HydrolibThrustGenerator
+        HydrolibThrustersControl
+        CMSIS
+    )
     target_include_directories(${LIBRARY_NAME} INTERFACE include)
-
+    add_subdirectory(example)
+
     message(STATUS "${LIBRARY_NAME} target added for MCU_FAMILY: ${MCU_FAMILY}")
 else()
     message(WARNING "${LIBRARY_NAME} target is not supported for MCU_FAMILY: ${MCU_FAMILY}")

hydrv_thruster/example/src/main.cpp

@@ -3,6 +3,7 @@
 #include "hydrv_tim_low.hpp"
 
 #include "hydrv_thruster.hpp"
+#include "hydrv_thrusters_control.hpp"
 
 extern "C"
 {
@@ -15,7 +16,7 @@ hydrv::timer::TimerLow tim(hydrv::timer::TimerLow::TIM5_low,
                            hydrv::thruster::Thruster::tim_prescaler,
                            hydrv::thruster::Thruster::tim_counter_period);
 
-hydrv::thruster::Thruster thruster(0, tim, tim_pin);
+hydrv::thruster::Thruster thruster(0, &tim, &tim_pin);
 
 int main(void)
 {

hydrv_thruster/include/hydrv_thruster.hpp

@@ -23,8 +23,8 @@ class Thruster
 
 public:
     constexpr Thruster(unsigned thruster_tim_channel,
-             hydrv::timer::TimerLow &thruster_tim,
-             hydrv::GPIO::GPIOLow &thruster_tim_pin);
+             hydrv::timer::TimerLow *thruster_tim,
+             hydrv::GPIO::GPIOLow *thruster_tim_pin);
 
     void Init();
 
@@ -35,36 +35,36 @@ class Thruster
     static constexpr unsigned SpeedToPWM_(int speed);
 
 private:
-    int speed;
+    unsigned tim_channel_;
+    timer::TimerLow &tim_;
+    GPIO::GPIOLow &tim_pin_;
 
-    timer::TimerLow &tim;
-    unsigned tim_channel;
-    GPIO::GPIOLow &tim_pin;
+    int speed_;
 };
 
 inline constexpr Thruster::Thruster(unsigned thruster_tim_channel,
-                          hydrv::timer::TimerLow &thruster_tim,
-                          hydrv::GPIO::GPIOLow &thruster_tim_pin)
-    : tim_channel(thruster_tim_channel), tim(thruster_tim),
-      tim_pin(thruster_tim_pin),
-      speed(speed_null)
+                          hydrv::timer::TimerLow *thruster_tim,
+                          hydrv::GPIO::GPIOLow *thruster_tim_pin)
+    : tim_channel_(thruster_tim_channel), tim_(*thruster_tim),
+      tim_pin_(*thruster_tim_pin),
+      speed_(speed_null)
 {
 }
 
 inline void Thruster::Init()
 {
-    tim.Init();
-    tim.ConfigurePWM(tim_channel, tim_pin);
-    tim.SetCaptureCompare(tim_channel, SpeedToPWM_(speed_null));
-    tim.StartTimer();
+    tim_.Init();
+    tim_.ConfigurePWM(tim_channel_, tim_pin_);
+    tim_.SetCaptureCompare(tim_channel_, SpeedToPWM_(speed_null));
+    tim_.StartTimer();
 }
 
 inline hydrolib_ReturnCode Thruster::SetSpeed(int thruster_speed)
 {
     if (thruster_speed <= max_speed && thruster_speed >= -max_speed)
     {
-        speed = thruster_speed;
-        tim.SetCaptureCompare(tim_channel, SpeedToPWM_(speed));
+        speed_ = thruster_speed;
+        tim_.SetCaptureCompare(tim_channel_, SpeedToPWM_(speed_));
         return HYDROLIB_RETURN_OK;
     }
     else
@@ -73,7 +73,7 @@ inline hydrolib_ReturnCode Thruster::SetSpeed(int thruster_speed)
     }
 }
 
-inline int Thruster::GetSpeed() { return speed; }
+inline int Thruster::GetSpeed() { return speed_; }
 
 inline constexpr unsigned Thruster::SpeedToPWM_(int speed)
 {

hydrv_thruster/include/hydrv_thrusters_control.hpp

@@ -0,0 +1,89 @@
+#pragma once
+
+#include "hydrolib_thrust_generator.hpp"
+#include "hydrolib_thrusters_control.hpp"
+#include "hydrv_thruster.hpp"
+
+namespace hydrv::thruster
+{
+
+template <unsigned THRUSTERS_COUNT>
+class ThrusterControl
+{
+private:
+    int x_rotation_gain_[THRUSTERS_COUNT];
+    int y_rotation_gain_[THRUSTERS_COUNT];
+    int z_rotation_gain_[THRUSTERS_COUNT];
+
+    int x_linear_gain_[THRUSTERS_COUNT];
+    int y_linear_gain_[THRUSTERS_COUNT];
+    int z_linear_gain_[THRUSTERS_COUNT];
+
+    int dest_[THRUSTERS_COUNT];
+
+public:
+    constexpr ThrusterControl(
+        int *x_rotation_gain, int *y_rotation_gain, int *z_rotation_gain,
+        int *x_linear_gain, int *y_linear_gain, int *z_linear_gain,
+        int single_clamp, int sum_clamp,
+        unsigned thruster_tim_channel[THRUSTERS_COUNT],
+        hydrv::timer::TimerLow *thruster_tim[THRUSTERS_COUNT],
+        hydrv::GPIO::GPIOLow *thruster_tim_pin[THRUSTERS_COUNT]);
+
+    void Init();
+    void SetControl(
+        hydrolib::controlling::ThrustersControlData thruster_control_data);
+
+private:
+    hydrv::thruster::Thruster thrusters_[THRUSTERS_COUNT];
+
+    hydrolib::controlling::ThrustGenerator<THRUSTERS_COUNT> thrust_generator_;
+
+private:
+    static_assert(hydrolib::controlling::ThrusterConcept<ThrusterControl>,
+                  "ThrusterControl must have SetControl()");
+};
+
+template <unsigned THRUSTERS_COUNT>
+inline constexpr ThrusterControl<THRUSTERS_COUNT>::ThrusterControl(
+    int *x_rotation_gain, int *y_rotation_gain, int *z_rotation_gain,
+    int *x_linear_gain, int *y_linear_gain, int *z_linear_gain,
+    int single_clamp, int sum_clamp,
+    unsigned thruster_tim_channel[THRUSTERS_COUNT],
+    hydrv::timer::TimerLow *thruster_tim[THRUSTERS_COUNT],
+    hydrv::GPIO::GPIOLow *thruster_tim_pin[THRUSTERS_COUNT])
+    : thrust_generator_{*x_rotation_gain, *y_rotation_gain, *z_rotation_gain,
+                        *x_linear_gain,   *y_linear_gain,   *z_linear_gain,
+                        single_clamp,     sum_clamp}
+{
+    {
+        for (int i = 0; i < THRUSTERS_COUNT; i++)
+        {
+            thrusters_[i](thruster_tim_channel[i], *thruster_tim[i],
+                          *thruster_tim_pin[i]);
+        }
+    }
+}
+
+template <unsigned THRUSTERS_COUNT>
+void ThrusterControl<THRUSTERS_COUNT>::Init()
+{
+    for (int i = 0; i < THRUSTERS_COUNT; i++)
+    {
+        thrusters_[i].Init();
+    }
+}
+
+template <unsigned THRUSTERS_COUNT>
+void ThrusterControl<THRUSTERS_COUNT>::SetControl(
+    hydrolib::controlling::ThrustersControlData thruster_control_data)
+{
+    thrust_generator_.ProcessWithFeedback(thruster_control_data, *dest_);
+
+    for (int i = 0; i < THRUSTERS_COUNT; i++)
+    {
+        thrusters_[i].SetSpeed(dest_[i]);
+    }
+}
+
+} // namespace hydrv::thruster