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