From 0cb754529776f0c0be909e08d1076bdd31c26e04 Mon Sep 17 00:00:00 2001
From: Fabrizio Monti <fabrizio.monti@skywarder.eu>
Date: Mon, 24 Mar 2025 22:40:20 +0100
Subject: [PATCH] [ENGINE] Updated actuators and can handler with Orion servo
valves.
---
src/Motor/Actuators/Actuators.cpp | 38 ++++++++++++++++----
src/Motor/Actuators/Actuators.h | 4 +--
src/Motor/CanHandler/CanHandler.cpp | 55 +++++++++++++++++++++++++----
src/common/CanConfig.h | 6 ++--
4 files changed, 86 insertions(+), 17 deletions(-)
diff --git a/src/Motor/Actuators/Actuators.cpp b/src/Motor/Actuators/Actuators.cpp
index 8582d035f..592999f9c 100644
--- a/src/Motor/Actuators/Actuators.cpp
+++ b/src/Motor/Actuators/Actuators.cpp
@@ -74,24 +74,46 @@ float Actuators::ServoInfo::getServoPosition()
Actuators::Actuators()
{
infos[0].servo = std::make_unique<Servo>(
- MIOSIX_SERVOS_1_TIM, TimerUtils::Channel::MIOSIX_SERVOS_1_CHANNEL,
+ MIOSIX_SERVOS_0_TIM, TimerUtils::Channel::MIOSIX_SERVOS_0_CHANNEL,
Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
Config::Servos::FREQUENCY);
infos[1].servo = std::make_unique<Servo>(
+ MIOSIX_SERVOS_1_TIM, TimerUtils::Channel::MIOSIX_SERVOS_1_CHANNEL,
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
+ infos[2].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_2_TIM, TimerUtils::Channel::MIOSIX_SERVOS_2_CHANNEL,
Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
Config::Servos::FREQUENCY);
+ infos[3].servo = std::make_unique<Servo>(
+ MIOSIX_SERVOS_3_TIM, TimerUtils::Channel::MIOSIX_SERVOS_3_CHANNEL,
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
ServoInfo* info;
info = getServo(ServosList::MAIN_VALVE);
info->limit = Config::Servos::MAIN_LIMIT;
info->flipped = Config::Servos::MAIN_FLIPPED;
info->unsafeSetServoPosition(0.0f);
+ // TODO: is the configuration correct? Same as lyra
+
+ info = getServo(ServosList::OX_VENTING_VALVE);
+ info->limit = Config::Servos::VENTING_LIMIT;
+ info->flipped = Config::Servos::VENTING_FLIPPED;
+ info->unsafeSetServoPosition(0.0f);
+ // TODO: is the configuration correct? Same as lyra
+
+ info = getServo(ServosList::NITROGEN_VALVE);
+ info->limit = Config::Servos::VENTING_LIMIT;
+ info->flipped = Config::Servos::VENTING_FLIPPED;
+ info->unsafeSetServoPosition(0.0f);
+ // TODO: is the configuration correct?
- info = getServo(ServosList::VENTING_VALVE);
+ info = getServo(ServosList::N2_QUENCHING_VALVE);
info->limit = Config::Servos::VENTING_LIMIT;
info->flipped = Config::Servos::VENTING_FLIPPED;
info->unsafeSetServoPosition(0.0f);
+ // TODO: is the configuration correct?
}
bool Actuators::start()
@@ -99,8 +121,8 @@ bool Actuators::start()
TaskScheduler& scheduler =
getModule<BoardScheduler>()->getActuatorsScheduler();
- infos[0].servo->enable();
- infos[1].servo->enable();
+ for (size_t i = 0; i < infos.size(); ++i)
+ infos[i].servo->enable();
// Reset all actions
lastActionTs = getTime();
@@ -165,10 +187,14 @@ Actuators::ServoInfo* Actuators::getServo(ServosList servo)
{
switch (servo)
{
- case VENTING_VALVE:
+ case OX_VENTING_VALVE:
return &infos[0];
case MAIN_VALVE:
return &infos[1];
+ case NITROGEN_VALVE:
+ return &infos[2];
+ case N2_QUENCHING_VALVE:
+ return &infos[3];
default:
// Oh FUCK
@@ -251,7 +277,7 @@ void Actuators::updatePositionsTask()
if (shouldVent)
{
// Open for at least timeout time
- openServoWithTime(ServosList::VENTING_VALVE,
+ openServoWithTime(ServosList::OX_VENTING_VALVE,
Config::Servos::SERVO_ACTION_TIMEOUT + 1000);
}
}
diff --git a/src/Motor/Actuators/Actuators.h b/src/Motor/Actuators/Actuators.h
index 15133ade1..2cfad5c92 100644
--- a/src/Motor/Actuators/Actuators.h
+++ b/src/Motor/Actuators/Actuators.h
@@ -25,7 +25,7 @@
#include <Motor/BoardScheduler.h>
#include <Motor/CanHandler/CanHandler.h>
#include <actuators/Servo/Servo.h>
-#include <common/MavlinkLyra.h>
+#include <common/MavlinkOrion.h>
#include <utils/DependencyManager/DependencyManager.h>
namespace Motor
@@ -76,7 +76,7 @@ private:
miosix::FastMutex infosMutex;
// Timestamp of last servo action
long long lastActionTs = 0;
- ServoInfo infos[2] = {};
+ std::array<ServoInfo, 4> infos;
};
} // namespace Motor
diff --git a/src/Motor/CanHandler/CanHandler.cpp b/src/Motor/CanHandler/CanHandler.cpp
index a110d856d..8a26ffb3d 100644
--- a/src/Motor/CanHandler/CanHandler.cpp
+++ b/src/Motor/CanHandler/CanHandler.cpp
@@ -100,18 +100,37 @@ bool CanHandler::start()
static_cast<uint8_t>(CanConfig::PrimaryType::SENSORS),
static_cast<uint8_t>(CanConfig::Board::MOTOR),
static_cast<uint8_t>(CanConfig::Board::BROADCAST),
- static_cast<uint8_t>(CanConfig::SensorId::TOP_TANK_PRESSURE),
+ static_cast<uint8_t>(CanConfig::SensorId::OX_TOP_TANK_PRESSURE),
static_cast<PressureData>(
- sensors->getTopTankPressLastSample()));
+ sensors->getOxTopTankPressLastSample()));
protocol.enqueueData(
static_cast<uint8_t>(CanConfig::Priority::HIGH),
static_cast<uint8_t>(CanConfig::PrimaryType::SENSORS),
static_cast<uint8_t>(CanConfig::Board::MOTOR),
static_cast<uint8_t>(CanConfig::Board::BROADCAST),
- static_cast<uint8_t>(CanConfig::SensorId::BOTTOM_TANK_PRESSURE),
+ static_cast<uint8_t>(
+ CanConfig::SensorId::OX_BOTTOM_TANK_PRESSURE_0),
static_cast<PressureData>(
- sensors->getBottomTankPressLastSample()));
+ sensors->getOxBottomTankPress0LastSample()));
+
+ protocol.enqueueData(
+ static_cast<uint8_t>(CanConfig::Priority::HIGH),
+ static_cast<uint8_t>(CanConfig::PrimaryType::SENSORS),
+ static_cast<uint8_t>(CanConfig::Board::MOTOR),
+ static_cast<uint8_t>(CanConfig::Board::BROADCAST),
+ static_cast<uint8_t>(
+ CanConfig::SensorId::OX_BOTTOM_TANK_PRESSURE_1),
+ static_cast<PressureData>(
+ sensors->getOxBottomTankPress1LastSample()));
+
+ protocol.enqueueData(
+ static_cast<uint8_t>(CanConfig::Priority::HIGH),
+ static_cast<uint8_t>(CanConfig::PrimaryType::SENSORS),
+ static_cast<uint8_t>(CanConfig::Board::MOTOR),
+ static_cast<uint8_t>(CanConfig::Board::BROADCAST),
+ static_cast<uint8_t>(CanConfig::SensorId::N2_TANK_PRESSURE),
+ static_cast<PressureData>(sensors->getN2TankPressLastSample()));
},
Config::CanHandler::PRESSURE_PERIOD);
@@ -172,11 +191,33 @@ bool CanHandler::start()
static_cast<uint8_t>(CanConfig::PrimaryType::ACTUATORS),
static_cast<uint8_t>(CanConfig::Board::MOTOR),
static_cast<uint8_t>(CanConfig::Board::BROADCAST),
- static_cast<uint8_t>(ServosList::VENTING_VALVE),
+ static_cast<uint8_t>(ServosList::OX_VENTING_VALVE),
+ ServoFeedback{
+ TimestampTimer::getTimestamp(),
+ actuators->getServoPosition(ServosList::OX_VENTING_VALVE),
+ actuators->isServoOpen(ServosList::OX_VENTING_VALVE)});
+
+ protocol.enqueueData(
+ static_cast<uint8_t>(CanConfig::Priority::HIGH),
+ static_cast<uint8_t>(CanConfig::PrimaryType::ACTUATORS),
+ static_cast<uint8_t>(CanConfig::Board::MOTOR),
+ static_cast<uint8_t>(CanConfig::Board::BROADCAST),
+ static_cast<uint8_t>(ServosList::NITROGEN_VALVE),
+ ServoFeedback{
+ TimestampTimer::getTimestamp(),
+ actuators->getServoPosition(ServosList::NITROGEN_VALVE),
+ actuators->isServoOpen(ServosList::NITROGEN_VALVE)});
+
+ protocol.enqueueData(
+ static_cast<uint8_t>(CanConfig::Priority::HIGH),
+ static_cast<uint8_t>(CanConfig::PrimaryType::ACTUATORS),
+ static_cast<uint8_t>(CanConfig::Board::MOTOR),
+ static_cast<uint8_t>(CanConfig::Board::BROADCAST),
+ static_cast<uint8_t>(ServosList::N2_QUENCHING_VALVE),
ServoFeedback{
TimestampTimer::getTimestamp(),
- actuators->getServoPosition(ServosList::VENTING_VALVE),
- actuators->isServoOpen(ServosList::VENTING_VALVE)});
+ actuators->getServoPosition(ServosList::N2_QUENCHING_VALVE),
+ actuators->isServoOpen(ServosList::N2_QUENCHING_VALVE)});
},
Config::CanHandler::ACTUATORS_PERIOD);
diff --git a/src/common/CanConfig.h b/src/common/CanConfig.h
index 579c5c920..60fff984b 100644
--- a/src/common/CanConfig.h
+++ b/src/common/CanConfig.h
@@ -77,8 +77,10 @@ enum class SensorId : uint8_t
PITOT_STATIC_PRESSURE,
PITOT_TOTAL_PRESSURE,
CC_PRESSURE,
- BOTTOM_TANK_PRESSURE,
- TOP_TANK_PRESSURE,
+ OX_BOTTOM_TANK_PRESSURE_0,
+ OX_BOTTOM_TANK_PRESSURE_1,
+ OX_TOP_TANK_PRESSURE,
+ N2_TANK_PRESSURE,
TANK_TEMPERATURE,
MOTOR_ACTUATORS_CURRENT,
MAIN_BOARD_CURRENT,
--
GitLab