From 31d5a05a03f4be53a91df655235e3adb933c29b4 Mon Sep 17 00:00:00 2001
From: Federico Lolli <federico.lolli@skywarder.eu>
Date: Sat, 11 May 2024 01:02:14 +0200
Subject: [PATCH] [ARP] Stepper manually manoeuvrable only in TEST state
Hub now interfaces to SMController and not Actuators directly
---
src/boards/Groundstation/Automated/Hub.cpp | 5 ++--
.../Automated/SMController/SMController.cpp | 28 +++++++++++++++++++
.../Automated/SMController/SMController.h | 20 +++++++++++--
3 files changed, 48 insertions(+), 5 deletions(-)
diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp
index 6ffb83f63..2f1ade0af 100644
--- a/src/boards/Groundstation/Automated/Hub.cpp
+++ b/src/boards/Groundstation/Automated/Hub.cpp
@@ -59,20 +59,19 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
float angle = mavlink_msg_set_stepper_angle_tc_get_angle(&msg);
// The stepper is moved of 'angle' degrees
- modules.get<Actuators>()->moveDeg(stepperId, angle);
+ modules.get<SMController>()->moveStepperDeg(stepperId, angle);
sendAck(msg);
break;
}
case MAVLINK_MSG_ID_SET_STEPPER_STEPS_TC:
{
- // TODO set in which state we can use this command
StepperList stepperId = static_cast<StepperList>(
mavlink_msg_set_stepper_steps_tc_get_stepper_id(&msg));
int16_t steps = mavlink_msg_set_stepper_steps_tc_get_steps(&msg);
// The stepper is moved of 'steps' steps
- modules.get<Actuators>()->move(stepperId, steps);
+ modules.get<SMController>()->moveStepperSteps(stepperId, steps);
sendAck(msg);
break;
}
diff --git a/src/boards/Groundstation/Automated/SMController/SMController.cpp b/src/boards/Groundstation/Automated/SMController/SMController.cpp
index 8de5dec26..5c366cf4d 100644
--- a/src/boards/Groundstation/Automated/SMController/SMController.cpp
+++ b/src/boards/Groundstation/Automated/SMController/SMController.cpp
@@ -22,6 +22,7 @@
#include "SMController.h"
+#include <Groundstation/Automated/Actuators/Actuators.h>
#include <Groundstation/Automated/Config/FollowerConfig.h>
#include <Groundstation/Automated/Config/PropagatorConfig.h>
#include <Groundstation/Automated/Config/SMControllerConfig.h>
@@ -111,6 +112,33 @@ void SMController::setInitialRocketCoordinates(
}
}
+void SMController::moveStepperDeg(StepperList stepperId, float angle)
+{
+ if (!testState(&SMController::state_test) &&
+ !testState(&SMController::state_test_nf))
+ {
+ LOG_ERR(logger, "Stepper can only be manually moved in the TEST state");
+ }
+ else
+ {
+ ModuleManager::getInstance().get<Actuators>()->moveDeg(stepperId,
+ angle);
+ }
+}
+
+void SMController::moveStepperSteps(StepperList stepperId, int16_t steps)
+{
+ if (!testState(&SMController::state_test) &&
+ !testState(&SMController::state_test_nf))
+ {
+ LOG_ERR(logger, "Stepper can only be manually moved in the TEST state");
+ }
+ else
+ {
+ ModuleManager::getInstance().get<Actuators>()->move(stepperId, steps);
+ }
+}
+
void SMController::update()
{
switch (status.state)
diff --git a/src/boards/Groundstation/Automated/SMController/SMController.h b/src/boards/Groundstation/Automated/SMController/SMController.h
index 37cf85856..2e41fda20 100644
--- a/src/boards/Groundstation/Automated/SMController/SMController.h
+++ b/src/boards/Groundstation/Automated/SMController/SMController.h
@@ -22,6 +22,7 @@
#pragma once
+#include <Groundstation/Automated/Actuators/Actuators.h>
#include <Groundstation/Automated/Follower/Follower.h>
#include <algorithms/NAS/NASState.h>
#include <algorithms/Propagator/Propagator.h>
@@ -76,12 +77,27 @@ public:
void setInitialRocketCoordinates(
const Boardcore::GPSData& antennaCoordinates);
- // Starts the FSM thread and adds an update function into the scheduler
+ /**
+ * @brief Starts the FSM thread and adds an update function into the
+ * scheduler
+ */
bool start() override;
- // Update routine called by the scheduler
+ /**
+ * @brief Update routine called by the scheduler
+ */
void update();
+ /**
+ * @brief move the stepper `stepperId` of `angle` degrees
+ */
+ void moveStepperDeg(StepperList stepperId, float angle);
+
+ /**
+ * @brief move the stepper `stepperId` of `steps` steps
+ */
+ void moveStepperSteps(StepperList stepperId, int16_t steps);
+
private:
/**
* @brief Logs the current state of the FSM
--
GitLab