diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 57a4927e6f94dd4a159dbf61a5dd597bca8fc853..cbdccca2a01c990308245275350930a16d9b4c34 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 0000000000000000000000000000000000000000..6e5d69fc82c487cc2accfdb6fb0e3669367fef75
--- /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 0000000000000000000000000000000000000000..008ffcf97a1bb6375d43415333dd273425ec3761
--- /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 0000000000000000000000000000000000000000..5d638d8b38c818e5b88f151b1298ced8b1a9ed3a
--- /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 0000000000000000000000000000000000000000..b29b1655b38128bc8e1ae10affcf9a4940a7e7c6
--- /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 fe43c27a9d05fdec48b0bdfad7e6ec8dfa2ec2b0..b4d51c19393586953d64ee0ebaa2a1d5f0553c8e 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 c8066ce51f830d4fbd51fd9a8da56ce6a66fea70..15f02567cdcbab1578b2fe93084f5689d18afc48 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 2a8a68ba1a6c1f0d54aa9019aa340f2f0d54de46..fddfb7c20d0955d6f7f4393567819efa998e2b52 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)