diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp index 6ffb83f63d9bb5fc4bae2328fc18589e8e3a9f1c..2f1ade0af87438b7777f60f7eb79ec2e3787137a 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 8de5dec2620daf80093d9759b3d76ff4c4b76032..5c366cf4d8b5d730139a97e4547d6d80c450e8c9 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 37cf858562ecd53bd3cdc1e266fa90783a056cda..2e41fda2061f1c0ee6e39fa630a98859aebb99de 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