From fe05fcdbff1bc093325204092a02828da28b6535 Mon Sep 17 00:00:00 2001
From: Davide <davide.mor@skywarder.eu>
Date: Mon, 26 Feb 2024 01:35:30 +0100
Subject: [PATCH] [RIGv2] Added Actuators

---
 cmake/dependencies.cmake                    |   1 +
 src/boards/RIGv2/Actuators/Actuators.cpp    | 344 ++++++++++++++++++++
 src/boards/RIGv2/Actuators/Actuators.h      |  91 ++++++
 src/boards/RIGv2/Actuators/ActuatorsData.h  |  56 ++++
 src/boards/RIGv2/Configs/ActuatorsConfig.h  |  62 ++++
 src/boards/RIGv2/Radio/Radio.cpp            |  48 ++-
 src/entrypoints/RIGv2/rig-v2-entry.cpp      |  30 +-
 src/scripts/logdecoder/RIGv2/logdecoder.cpp |   2 +
 8 files changed, 618 insertions(+), 16 deletions(-)
 create mode 100644 src/boards/RIGv2/Actuators/Actuators.cpp
 create mode 100644 src/boards/RIGv2/Actuators/Actuators.h
 create mode 100644 src/boards/RIGv2/Actuators/ActuatorsData.h
 create mode 100644 src/boards/RIGv2/Configs/ActuatorsConfig.h

diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 57a4927e6..cbdccca2a 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -71,6 +71,7 @@ set(RIG_COMPUTER
 set(RIG_V2_COMPUTER
     src/boards/RIGv2/Radio/Radio.cpp
     src/boards/RIGv2/Sensors/Sensors.cpp
+    src/boards/RIGv2/Actuators/Actuators.cpp
 )
 
 set(CON_RIG_COMPUTER
diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
new file mode 100644
index 000000000..6e5d69fc8
--- /dev/null
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -0,0 +1,344 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Actuators.h"
+
+#include <RIGv2/Actuators/ActuatorsData.h>
+#include <RIGv2/Configs/ActuatorsConfig.h>
+#include <common/Events.h>
+// TODO(davide.mor): Remove TimestampTimer
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Boardcore;
+using namespace miosix;
+using namespace Common;
+using namespace RIGv2;
+
+Actuators::Actuators(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler}
+{
+    // Initialize servos
+    infos[0].servo = std::make_unique<Servo>(
+        MIOSIX_SERVOS_1_TIM, TimerUtils::Channel::MIOSIX_SERVOS_1_CHANNEL,
+        Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    infos[1].servo = std::make_unique<Servo>(
+        MIOSIX_SERVOS_2_TIM, TimerUtils::Channel::MIOSIX_SERVOS_2_CHANNEL,
+        Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    infos[2].servo = std::make_unique<Servo>(
+        MIOSIX_SERVOS_3_TIM, TimerUtils::Channel::MIOSIX_SERVOS_3_CHANNEL,
+        Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    infos[3].servo = std::make_unique<Servo>(
+        MIOSIX_SERVOS_4_TIM, TimerUtils::Channel::MIOSIX_SERVOS_4_CHANNEL,
+        Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    // This servo is currently unusable, due to it sharing the same timer as
+    // miosix, TIM5 infos[4].servo = std::make_unique<Servo>(
+    //     MIOSIX_SERVOS_5_TIM, TimerUtils::Channel::MIOSIX_SERVOS_5_CHANNEL,
+    //     Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    infos[5].servo = std::make_unique<Servo>(
+        MIOSIX_SERVOS_6_TIM, TimerUtils::Channel::MIOSIX_SERVOS_6_CHANNEL,
+        Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    infos[6].servo = std::make_unique<Servo>(
+        MIOSIX_SERVOS_7_TIM, TimerUtils::Channel::MIOSIX_SERVOS_7_CHANNEL,
+        Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    // This servo is currently unusable, due to it sharing the same timer as
+    // servo 1 infos[7].servo = std::make_unique<Servo>(
+    //     MIOSIX_SERVOS_8_TIM, TimerUtils::Channel::MIOSIX_SERVOS_8_CHANNEL,
+    //     Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    infos[8].servo = std::make_unique<Servo>(
+        MIOSIX_SERVOS_9_TIM, TimerUtils::Channel::MIOSIX_SERVOS_9_CHANNEL,
+        Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    infos[9].servo = std::make_unique<Servo>(
+        MIOSIX_SERVOS_10_TIM, TimerUtils::Channel::MIOSIX_SERVOS_10_CHANNEL,
+        Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+
+    ServoInfo *info;
+    info               = getServo(ServosList::FILLING_VALVE);
+    info->maxAperture  = Config::Servos::DEFAULT_FILLING_MAXIMUM_APERTURE;
+    info->openingTime  = Config::Servos::DEFAULT_FILLING_OPENING_TIME;
+    info->flipped      = Config::Servos::FILLING_FLIPPED;
+    info->openingEvent = Common::Events::MOTOR_OPEN_FILLING_VALVE;
+    info->closingEvent = Common::Events::MOTOR_CLOSE_FILLING_VALVE;
+
+    info               = getServo(ServosList::RELEASE_VALVE);
+    info->maxAperture  = Config::Servos::DEFAULT_RELEASE_MAXIMUM_APERTURE;
+    info->openingTime  = Config::Servos::DEFAULT_RELEASE_OPENING_TIME;
+    info->flipped      = Config::Servos::RELEASE_FLIPPED;
+    info->openingEvent = Common::Events::MOTOR_OPEN_RELEASE_VALVE;
+    info->closingEvent = Common::Events::MOTOR_CLOSE_RELEASE_VALVE;
+
+    info               = getServo(ServosList::DISCONNECT_SERVO);
+    info->maxAperture  = Config::Servos::DEFAULT_DISCONNECT_MAXIMUM_APERTURE;
+    info->openingTime  = Config::Servos::DEFAULT_DISCONNECT_OPENING_TIME;
+    info->flipped      = Config::Servos::DISCONNECT_FLIPPED;
+    info->openingEvent = Common::Events::MOTOR_DISCONNECT;
+
+    info               = getServo(ServosList::MAIN_VALVE);
+    info->maxAperture  = Config::Servos::DEFAULT_MAIN_MAXIMUM_APERTURE;
+    info->openingTime  = Config::Servos::DEFAULT_MAIN_OPENING_TIME;
+    info->flipped      = Config::Servos::MAIN_FLIPPED;
+    info->openingEvent = Common::Events::MOTOR_OPEN_FEED_VALVE;
+    info->closingEvent = Common::Events::MOTOR_CLOSE_FEED_VALVE;
+
+    // This servo is not yet enabled
+    // info               = getServo(ServosList::VENTING_VALVE);
+    // info->maxAperture  = Config::Servos::DEFAULT_VENTING_MAXIMUM_APERTURE;
+    // info->openingTime  = Config::Servos::DEFAULT_VENTING_OPENING_TIME;
+    // info->flipped      = true;
+    // info->openingEvent = Common::Events::MOTOR_OPEN_VENTING_VALVE;
+    // info->closingEvent = Common::Events::MOTOR_CLOSE_VENTING_VALVE;
+}
+
+bool Actuators::start()
+{
+    infos[0].servo->enable();
+    infos[1].servo->enable();
+    infos[2].servo->enable();
+    infos[3].servo->enable();
+    // infos[4].servo->enable();
+    infos[5].servo->enable();
+    infos[6].servo->enable();
+    // infos[7].servo->enable();
+    infos[8].servo->enable();
+    infos[9].servo->enable();
+
+    size_t result =
+        scheduler.addTask([this]() { updatePositionsTask(); },
+                          Config::Servos::SERVO_TIMINGS_CHECK_PERIOD);
+    if (result == 0)
+    {
+        LOG_ERR(logger, "Failed to add updatePositionsTask");
+        return false;
+    }
+
+    return true;
+}
+
+void Actuators::stop()
+{
+    infos[0].servo->disable();
+    infos[1].servo->disable();
+    infos[2].servo->disable();
+    infos[3].servo->disable();
+    // infos[4].servo->disable();
+    infos[5].servo->disable();
+    infos[6].servo->disable();
+    // infos[7].servo->disable();
+    infos[8].servo->disable();
+    infos[9].servo->disable();
+
+    // We just can only hope the scheduler is also disabled, otherwise bad
+    // things will happen
+}
+
+bool Actuators::wiggleServo(ServosList servo)
+{
+    // Wiggle means open the servo for 1s
+    return openServoAtomic(servo, 1000);
+}
+
+bool Actuators::toggleServo(ServosList servo)
+{
+    Lock<FastMutex> lock(infosMutex);
+    ServoInfo *info = getServo(servo);
+    if (info == nullptr)
+    {
+        return false;
+    }
+
+    if (info->closeTs == 0)
+    {
+        // The servo is closed, open it
+        openServoInner(info, info->openingTime);
+    }
+    else
+    {
+        // The servo is open, close it
+        closeServoInner(info);
+    }
+
+    return true;
+}
+
+bool Actuators::closeServo(ServosList servo)
+{
+    Lock<FastMutex> lock(infosMutex);
+    ServoInfo *info = getServo(servo);
+    if (info == nullptr)
+    {
+        return false;
+    }
+
+    closeServoInner(info);
+    return true;
+}
+
+bool Actuators::openServoAtomic(ServosList servo, uint64_t time)
+{
+    Lock<FastMutex> lock(infosMutex);
+    ServoInfo *info = getServo(servo);
+    if (info == nullptr)
+    {
+        return false;
+    }
+
+    openServoInner(info, time);
+    return true;
+}
+
+bool Actuators::setMaxAperture(ServosList servo, float aperture)
+{
+    Lock<FastMutex> lock(infosMutex);
+    ServoInfo *info = getServo(servo);
+    if (info == nullptr)
+    {
+        return false;
+    }
+
+    info->maxAperture = aperture;
+    return true;
+}
+
+bool Actuators::setOpeningTime(ServosList servo, uint64_t time)
+{
+    Lock<FastMutex> lock(infosMutex);
+    ServoInfo *info = getServo(servo);
+    if (info == nullptr)
+    {
+        return false;
+    }
+
+    info->openingTime = time;
+    return true;
+}
+
+void Actuators::openServoInner(ServoInfo *info, uint64_t time)
+{
+    long long currentTime = getTime();
+
+    info->openedTs = currentTime;
+    info->closeTs  = currentTime + (time * Constants::NS_IN_MS);
+
+    // TODO(davide.mor): Dispatch the open event
+}
+
+void Actuators::closeServoInner(ServoInfo *info)
+{
+    info->closeTs = 0;
+
+    // TODO(davide.mor): Dispatch the close event
+}
+
+Actuators::ServoInfo *Actuators::getServo(ServosList servo)
+{
+    switch (servo)
+    {
+        case FILLING_VALVE:
+            return &infos[3];
+        case RELEASE_VALVE:
+            return &infos[2];
+        case DISCONNECT_SERVO:
+            return &infos[1];
+        case MAIN_VALVE:
+            return &infos[0];
+            // TODO(davide.mor): Decide this servo
+            // case VENTING_VALVE:
+            //     return &infos[8];
+
+        default:
+            // Oh FUCK
+            LOG_ERR(logger, "Invalid servo requested");
+            return nullptr;
+    }
+}
+
+void Actuators::unsafeSetServoPosition(uint8_t idx, float position)
+{
+    // Invert the position if the servo is flipped
+    if (infos[idx].flipped)
+    {
+        infos[idx].servo->setPosition(1.0 - position);
+    }
+    else
+    {
+        infos[idx].servo->setPosition(position);
+    }
+
+    // Log the update
+    ActuatorsData data;
+    data.timestamp = TimestampTimer::getTimestamp();
+    data.servoIdx  = idx;
+    data.position  = position;
+    sdLogger.log(data);
+}
+
+void Actuators::updatePositionsTask()
+{
+    Lock<FastMutex> lock(infosMutex);
+
+    long long currentTime = getTime();
+
+    // Iterate over all servos
+    for (uint8_t idx = 0; idx < 10; idx++)
+    {
+        if (currentTime < infos[idx].closeTs)
+        {
+            // The valve should be open
+            if (currentTime <
+                infos[idx].openedTs + (Config::Servos::SERVO_CONFIDENCE_TIME *
+                                       Constants::NS_IN_MS))
+            {
+                // We should open the valve all the way
+                unsafeSetServoPosition(idx, infos[idx].maxAperture);
+            }
+            else
+            {
+                // Time to wiggle the valve a little
+                unsafeSetServoPosition(
+                    idx, infos[idx].maxAperture *
+                             (1.0 - Config::Servos::SERVO_CONFIDENCE));
+            }
+        }
+        else
+        {
+            // Ok the valve should be closed
+            if (infos[idx].closeTs != 0)
+            {
+                // The valve JUST closed, notify everybody
+                infos[idx].closeTs = 0;
+
+                // TODO(davide.mor): Dispatch the close event
+            }
+
+            if (currentTime <
+                infos[idx].closeTs + (Config::Servos::SERVO_CONFIDENCE_TIME *
+                                      Constants::NS_IN_MS))
+            {
+                // We should close the valve all the way
+                unsafeSetServoPosition(idx, 0.0);
+            }
+            else
+            {
+                // Time to wiggle the valve a little
+                unsafeSetServoPosition(idx, Config::Servos::SERVO_CONFIDENCE);
+            }
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h
new file mode 100644
index 000000000..008ffcf97
--- /dev/null
+++ b/src/boards/RIGv2/Actuators/Actuators.h
@@ -0,0 +1,91 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <actuators/Servo/Servo.h>
+#include <common/Mavlink.h>
+#include <miosix.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <memory>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIGv2
+{
+
+class Actuators : public Boardcore::Module
+{
+private:
+    struct ServoInfo
+    {
+        std::unique_ptr<Boardcore::Servo> servo;
+        // How much time to stay open
+        uint64_t openingTime = 100000;  // Default 100s [ms]
+        // What angle is the maximum
+        float maxAperture = 1.0;
+        // Should this servo be reversed?
+        bool flipped = false;
+
+        // What event to fire while opening?
+        uint8_t openingEvent = 0;
+        // What event to fire while closing?
+        uint8_t closingEvent = 0;
+
+        // Timestamp of when the servo should close, 0 if closed
+        long long closeTs = 0;
+        // Timestamp of when the servo was opened
+        long long openedTs = 0;
+    };
+
+public:
+    Actuators(Boardcore::TaskScheduler &scheduler);
+
+    [[nodiscard]] bool start();
+
+    void stop();
+
+    bool wiggleServo(ServosList servo);
+    bool toggleServo(ServosList servo);
+    bool closeServo(ServosList servo);
+    bool openServoAtomic(ServosList servo, uint64_t time);
+    bool setMaxAperture(ServosList servo, float aperture);
+    bool setOpeningTime(ServosList servo, uint64_t time);
+
+private:
+    ServoInfo *getServo(ServosList servo);
+
+    void openServoInner(ServoInfo *info, uint64_t time);
+    void closeServoInner(ServoInfo *info);
+    void unsafeSetServoPosition(uint8_t idx, float position);
+    void updatePositionsTask();
+
+    Boardcore::Logger &sdLogger   = Boardcore::Logger::getInstance();
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("actuators");
+
+    Boardcore::TaskScheduler &scheduler;
+
+    miosix::FastMutex infosMutex;
+    ServoInfo infos[10] = {};
+};
+
+}  // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/Actuators/ActuatorsData.h b/src/boards/RIGv2/Actuators/ActuatorsData.h
new file mode 100644
index 000000000..5d638d8b3
--- /dev/null
+++ b/src/boards/RIGv2/Actuators/ActuatorsData.h
@@ -0,0 +1,56 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <ostream>
+
+namespace RIGv2
+{
+struct ActuatorsData
+{
+    uint64_t timestamp;
+    uint8_t servoIdx;
+    float position;
+
+    ActuatorsData()
+    {
+        timestamp = 0;
+        servoIdx  = 0;
+        position  = 0;
+    }
+
+    ActuatorsData(uint64_t time, uint8_t servoIdx, float pos)
+        : timestamp(time), servoIdx(servoIdx), position(pos)
+    {
+    }
+
+    static std::string header() { return "timestamp,servoIdx,position\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)servoIdx << "," << position << "\n";
+    }
+};
+
+}  // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/Configs/ActuatorsConfig.h b/src/boards/RIGv2/Configs/ActuatorsConfig.h
new file mode 100644
index 000000000..b29b1655b
--- /dev/null
+++ b/src/boards/RIGv2/Configs/ActuatorsConfig.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <interfaces-impl/hwmapping.h>
+
+namespace RIGv2
+{
+namespace Config
+{
+
+namespace Servos
+{
+
+static constexpr unsigned int MIN_PULSE = 500;
+static constexpr unsigned int MAX_PULSE = 2500;
+
+static constexpr uint32_t SERVO_TIMINGS_CHECK_PERIOD = 100;
+static constexpr long long SERVO_CONFIDENCE_TIME     = 500;       // 0.5s
+static constexpr float SERVO_CONFIDENCE              = 1 / 50.0;  // 2%
+
+static constexpr uint32_t DEFAULT_FILLING_OPENING_TIME    = 15000;  // 15s
+static constexpr uint32_t DEFAULT_VENTING_OPENING_TIME    = 15000;  // 15s
+static constexpr uint32_t DEFAULT_MAIN_OPENING_TIME       = 6000;   // 6s
+static constexpr uint32_t DEFAULT_RELEASE_OPENING_TIME    = 10000;  // 10s
+static constexpr uint32_t DEFAULT_DISCONNECT_OPENING_TIME = 10000;  // 10s
+
+static constexpr float DEFAULT_FILLING_MAXIMUM_APERTURE    = 1.00f;
+static constexpr float DEFAULT_VENTING_MAXIMUM_APERTURE    = 0.80f;
+static constexpr float DEFAULT_MAIN_MAXIMUM_APERTURE       = 0.87f;
+static constexpr float DEFAULT_RELEASE_MAXIMUM_APERTURE    = 1.0f;
+static constexpr float DEFAULT_DISCONNECT_MAXIMUM_APERTURE = 0.110f;
+
+static constexpr bool FILLING_FLIPPED    = true;
+static constexpr bool VENTING_FLIPPED    = true;
+static constexpr bool MAIN_FLIPPED       = true;
+static constexpr bool RELEASE_FLIPPED    = false;
+static constexpr bool DISCONNECT_FLIPPED = true;
+
+}  // namespace Servos
+}  // namespace Config
+}  // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index fe43c27a9..b4d51c193 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -22,6 +22,7 @@
 
 #include "Radio.h"
 
+#include <RIGv2/Actuators/Actuators.h>
 #include <RIGv2/Buses.h>
 #include <RIGv2/Sensors/Sensors.h>
 #include <common/Radio.h>
@@ -160,6 +161,7 @@ Boardcore::MavlinkStatus Radio::getMavStatus()
 
 void Radio::handleMessage(const mavlink_message_t& msg)
 {
+    ModuleManager& modules = ModuleManager::getInstance();
     switch (msg.msgid)
     {
         case MAVLINK_MSG_ID_PING_TC:
@@ -200,28 +202,60 @@ void Radio::handleMessage(const mavlink_message_t& msg)
 
         case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
         {
-            // TODO: Implement servo wiggle
-            sendNack(msg);
+            ServosList servo = static_cast<ServosList>(
+                mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
+
+            if (modules.get<Actuators>()->wiggleServo(servo))
+            {
+                sendAck(msg);
+            }
+            else
+            {
+                sendNack(msg);
+            }
             break;
         }
 
         case MAVLINK_MSG_ID_SET_ATOMIC_VALVE_TIMING_TC:
         {
-            // TODO: Implement set atomic valve timing
-            sendNack(msg);
+            uint32_t time =
+                mavlink_msg_set_atomic_valve_timing_tc_get_maximum_timing(&msg);
+            ServosList servo = static_cast<ServosList>(
+                mavlink_msg_set_atomic_valve_timing_tc_get_servo_id(&msg));
+
+            if (modules.get<Actuators>()->setOpeningTime(servo, time))
+            {
+                sendAck(msg);
+            }
+            else
+            {
+                sendNack(msg);
+            }
             break;
         }
 
         case MAVLINK_MSG_ID_SET_VALVE_MAXIMUM_APERTURE_TC:
         {
-            // TODO: Implement set valve maximum aperture
-            sendNack(msg);
+            float aperture =
+                mavlink_msg_set_valve_maximum_aperture_tc_get_maximum_aperture(
+                    &msg);
+            ServosList servo = static_cast<ServosList>(
+                mavlink_msg_set_valve_maximum_aperture_tc_get_servo_id(&msg));
+
+            if (modules.get<Actuators>()->setMaxAperture(servo, aperture))
+            {
+                sendAck(msg);
+            }
+            else
+            {
+                sendNack(msg);
+            }
             break;
         }
 
         case MAVLINK_MSG_ID_SET_IGNITION_TIME_TC:
         {
-            // TODO: Implement set ignition time
+            // TODO(davide.mor): Implement set ignition time
             sendNack(msg);
             break;
         }
diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp
index c8066ce51..15f02567c 100644
--- a/src/entrypoints/RIGv2/rig-v2-entry.cpp
+++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp
@@ -20,6 +20,7 @@
  * THE SOFTWARE.
  */
 
+#include <RIGv2/Actuators/Actuators.h>
 #include <RIGv2/Buses.h>
 #include <RIGv2/Radio/Radio.h>
 #include <RIGv2/Sensors/Sensors.h>
@@ -37,11 +38,13 @@ int main()
     PrintLogger logger     = Logging::getLogger("main");
 
     // TODO: Move this to a dedicated board scheduler
-    TaskScheduler *scheduler = new TaskScheduler(3);
+    TaskScheduler *scheduler1 = new TaskScheduler(3);
+    TaskScheduler *scheduler2 = new TaskScheduler(4);
 
-    Buses *buses     = new Buses();
-    Sensors *sensors = new Sensors(*scheduler);
-    Radio *radio     = new Radio();
+    Buses *buses         = new Buses();
+    Sensors *sensors     = new Sensors(*scheduler1);
+    Actuators *actuators = new Actuators(*scheduler2);
+    Radio *radio         = new Radio();
 
     Logger &sdLogger = Logger::getInstance();
 
@@ -60,6 +63,12 @@ int main()
         LOG_ERR(logger, "Error failed to insert Sensors");
     }
 
+    if (!modules.insert<Actuators>(actuators))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error failed to insert Actuators");
+    }
+
     if (!modules.insert<Radio>(radio))
     {
         initResult = false;
@@ -73,13 +82,19 @@ int main()
         LOG_ERR(logger, "Error failed to start Sensors module");
     }
 
+    if (!actuators->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error failed to start Actuators module");
+    }
+
     if (!radio->start())
     {
         initResult = false;
         LOG_ERR(logger, "Error failed to start Radio module");
     }
 
-    if (!scheduler->start())
+    if (!scheduler1->start() || !scheduler2->start())
     {
         initResult = false;
         LOG_ERR(logger, "Error failed to start scheduler");
@@ -87,13 +102,10 @@ int main()
 
     if (!initResult)
     {
+        // TODO(davide.mor): What to do in case of not good?
         LOG_INFO(logger, "All good!");
     }
 
-    // TEMPORARY CODE
-    // Start logger module
-    sdLogger.start();
-
     // Periodic statistics
     while (true)
     {
diff --git a/src/scripts/logdecoder/RIGv2/logdecoder.cpp b/src/scripts/logdecoder/RIGv2/logdecoder.cpp
index 2a8a68ba1..fddfb7c20 100644
--- a/src/scripts/logdecoder/RIGv2/logdecoder.cpp
+++ b/src/scripts/logdecoder/RIGv2/logdecoder.cpp
@@ -21,6 +21,7 @@
  */
 
 #include <RIGv2/Sensors/SensorsData.h>
+#include <RIGv2/Actuators/ActuatorsData.h>
 #include <logger/Deserializer.h>
 #include <logger/LogTypes.h>
 #include <tscpp/stream.h>
@@ -51,6 +52,7 @@ void registerTypes(Deserializer& ds)
 
     // Custom types
     ds.registerType<ADCsData>();
+    ds.registerType<ActuatorsData>();
 }
 
 void showUsage(const string& cmdName)
-- 
GitLab