diff --git a/src/boards/Groundstation/Automated/Actuators/Actuators.cpp b/src/boards/Groundstation/Automated/Actuators/Actuators.cpp
index 3899485f7c8f0801d273f0d2b6170c06094d1fa5..2b69c619843b5ff7f9e6d0bca12ea1129ccd98ec 100644
--- a/src/boards/Groundstation/Automated/Actuators/Actuators.cpp
+++ b/src/boards/Groundstation/Automated/Actuators/Actuators.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Emilio Corigliano
+/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -151,7 +151,7 @@ float Actuators::getCurrentDegPosition(StepperList axis)
return 0;
}
-void Actuators::move(StepperList axis, int16_t steps)
+ErrorMovement Actuators::move(StepperList axis, int16_t steps)
{
float microstepping = 1.0;
float step_angle = 1.8;
@@ -170,10 +170,11 @@ void Actuators::move(StepperList axis, int16_t steps)
assert(false && "Non existent stepper");
break;
}
- moveDeg(axis, static_cast<float>(steps) * step_angle / microstepping);
+ return moveDeg(axis,
+ static_cast<float>(steps) * step_angle / microstepping);
}
-void Actuators::moveDeg(StepperList axis, float degrees)
+ErrorMovement Actuators::moveDeg(StepperList axis, float degrees)
{
if (emergencyStop)
{
@@ -192,17 +193,24 @@ void Actuators::moveDeg(StepperList axis, float degrees)
break;
}
- return;
+ return ErrorMovement::EMERGENCY_STOP;
}
+ ErrorMovement actuationState =
+ ErrorMovement::OK; //< In case the move command is not limited
float positionDeg = getCurrentDegPosition(axis);
switch (axis)
{
case StepperList::STEPPER_X:
+
+ if (!stepperX.isEnabled())
+ return ErrorMovement::DISABLED;
+
// LIMIT POSITION IN ACCEPTABLE RANGE
if (positionDeg + degrees > Config::MAX_ANGLE_HORIZONTAL)
{
- degrees = Config::MAX_ANGLE_HORIZONTAL - positionDeg;
+ degrees = Config::MAX_ANGLE_HORIZONTAL - positionDeg;
+ actuationState = ErrorMovement::LIMIT;
}
else if (positionDeg + degrees < Config::MIN_ANGLE_HORIZONTAL)
{
@@ -215,10 +223,15 @@ void Actuators::moveDeg(StepperList axis, float degrees)
deltaX = degrees;
break;
case StepperList::STEPPER_Y:
+
+ if (!stepperY.isEnabled())
+ return ErrorMovement::DISABLED;
+
// LIMIT POSITION IN ACCEPTABLE RANGE
if (positionDeg + degrees > Config::MAX_ANGLE_VERTICAL)
{
- degrees = Config::MAX_ANGLE_VERTICAL - positionDeg;
+ degrees = Config::MAX_ANGLE_VERTICAL - positionDeg;
+ actuationState = ErrorMovement::LIMIT;
}
else if (positionDeg + degrees < Config::MIN_ANGLE_VERTICAL)
{
@@ -232,11 +245,13 @@ void Actuators::moveDeg(StepperList axis, float degrees)
break;
default:
assert(false && "Non existent stepper");
+ actuationState = ErrorMovement::NO_STEPPER;
break;
}
+ return actuationState;
}
-void Actuators::setPosition(StepperList axis, int16_t steps)
+ErrorMovement Actuators::setPosition(StepperList axis, int16_t steps)
{
float microstepping = 1.0;
@@ -244,25 +259,30 @@ void Actuators::setPosition(StepperList axis, int16_t steps)
switch (axis)
{
case StepperList::STEPPER_X:
+ if (!stepperX.isEnabled())
+ return ErrorMovement::DISABLED;
microstepping =
static_cast<float>(Config::HORIZONTAL_MICROSTEPPING);
step_angle = Config::HORIZONTAL_STEP_ANGLE;
break;
case StepperList::STEPPER_Y:
+ if (!stepperY.isEnabled())
+ return ErrorMovement::DISABLED;
microstepping = static_cast<float>(Config::VERTICAL_MICROSTEPPING);
step_angle = Config::VERTICAL_STEP_ANGLE;
break;
default:
assert(false && "Non existent stepper");
+ return ErrorMovement::NO_STEPPER;
break;
}
- setPositionDeg(axis,
- static_cast<float>(steps) * step_angle / microstepping);
+ return setPositionDeg(
+ axis, static_cast<float>(steps) * step_angle / microstepping);
}
-void Actuators::setPositionDeg(StepperList axis, float positionDeg)
+ErrorMovement Actuators::setPositionDeg(StepperList axis, float positionDeg)
{
- moveDeg(axis, positionDeg - getCurrentDegPosition(axis));
+ return moveDeg(axis, positionDeg - getCurrentDegPosition(axis));
}
void Actuators::IRQemergencyStop()
diff --git a/src/boards/Groundstation/Automated/Actuators/Actuators.h b/src/boards/Groundstation/Automated/Actuators/Actuators.h
index 84243dac0c5b368bd076ba191c9244b71f00056e..c78b9e7ab3614c0a0671280c8d62c56e282a7f21 100644
--- a/src/boards/Groundstation/Automated/Actuators/Actuators.h
+++ b/src/boards/Groundstation/Automated/Actuators/Actuators.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Emilio Corigliano
+/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -39,6 +39,20 @@
namespace Antennas
{
+
+/**
+ * @brief Error handling enum for the stepper movement
+ */
+enum class ErrorMovement : uint8_t
+{
+ OK, ///< `0`
+ LIMIT, ///< `1` The STEPPER_X reached its actuation limits
+ NOT_TEST, ///< `2` Such movement is allowed only in test
+ NO_STEPPER, ///< `3` The specified stepper does not exist
+ DISABLED, ///< `4`
+ EMERGENCY_STOP, ///< `5`
+};
+
class Actuators : public Boardcore::Module
{
public:
@@ -54,10 +68,10 @@ public:
void setSpeed(StepperList axis, float speed);
- void move(StepperList axis, int16_t steps);
- void moveDeg(StepperList axis, float degrees);
- void setPosition(StepperList axis, int16_t steps);
- void setPositionDeg(StepperList axis, float degrees);
+ ErrorMovement move(StepperList axis, int16_t steps);
+ ErrorMovement moveDeg(StepperList axis, float degrees);
+ ErrorMovement setPosition(StepperList axis, int16_t steps);
+ ErrorMovement setPositionDeg(StepperList axis, float degrees);
void zeroPosition();
diff --git a/src/boards/Groundstation/Automated/Follower/Follower.cpp b/src/boards/Groundstation/Automated/Follower/Follower.cpp
index cfc0c96d22f2cd0cff389e5b9e2eaeb98762749a..0a5276059b8837df921f4f07e2eea5c7334733c3 100644
--- a/src/boards/Groundstation/Automated/Follower/Follower.cpp
+++ b/src/boards/Groundstation/Automated/Follower/Follower.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Emilio Corigliano, Niccolò Betto, Federico Lolli
+ * Author: Emilio Corigliano, Niccolò Betto, Federico Lolli, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -152,10 +152,24 @@ void Follower::step()
StepperList::STEPPER_Y, verticalSpeed);
// Actuating steppers
- ModuleManager::getInstance().get<Actuators>()->moveDeg(
- StepperList::STEPPER_X, stepperAngles.yaw);
- ModuleManager::getInstance().get<Actuators>()->moveDeg(
+ ErrorMovement actuation =
+ ModuleManager::getInstance().get<Actuators>()->moveDeg(
+ StepperList::STEPPER_X, stepperAngles.yaw);
+
+ if (actuation != ErrorMovement::OK)
+ LOG_ERR(logger,
+ "Step antenna - STEPPER_X could not move or reached move "
+ "limit. Error: ",
+ actuation, "\n");
+
+ actuation = ModuleManager::getInstance().get<Actuators>()->moveDeg(
StepperList::STEPPER_Y, stepperAngles.pitch);
+
+ if (actuation != ErrorMovement::OK)
+ LOG_ERR(logger,
+ "Step antenna - STEPPER_Y could not move or reached move "
+ "limit. Error: ",
+ actuation, "\n");
}
AntennaAngles Follower::rocketPositionToAntennaAngles(
diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp
index d1b3d9845b82ac537d09bf514bd22d968b062e65..caa0dd88a2ccda4bce3278bec1ac85865aa3e223 100644
--- a/src/boards/Groundstation/Automated/Hub.cpp
+++ b/src/boards/Groundstation/Automated/Hub.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Davide Mor, Federico Lolli
+ * Author: Davide Mor, Federico Lolli, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -94,8 +94,12 @@ 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<SMController>()->moveStepperDeg(stepperId, angle);
- sendAck(msg);
+ ErrorMovement moved =
+ modules.get<SMController>()->moveStepperDeg(stepperId, angle);
+ if (moved == ErrorMovement::OK)
+ sendAck(msg);
+ else
+ sendNack(msg);
break;
}
case MAVLINK_MSG_ID_SET_STEPPER_STEPS_TC:
@@ -106,8 +110,12 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
int16_t steps = mavlink_msg_set_stepper_steps_tc_get_steps(&msg);
// The stepper is moved of 'steps' steps
- modules.get<SMController>()->moveStepperSteps(stepperId, steps);
- sendAck(msg);
+ ErrorMovement moved =
+ modules.get<SMController>()->moveStepperSteps(stepperId, steps);
+ if (moved == ErrorMovement::OK)
+ sendAck(msg);
+ else
+ sendNack(msg);
break;
}
case MAVLINK_MSG_ID_SET_ROCKET_COORDINATES_ARP_TC:
diff --git a/src/boards/Groundstation/Automated/SMController/SMController.cpp b/src/boards/Groundstation/Automated/SMController/SMController.cpp
index 58676053ac4fee294d5eeb8cbccb2977fc580add..b6204506871cfa0ce1e46ff0e2edc75ff36a2a10 100644
--- a/src/boards/Groundstation/Automated/SMController/SMController.cpp
+++ b/src/boards/Groundstation/Automated/SMController/SMController.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Federico Lolli
+ * Author: Federico Lolli, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -112,30 +112,34 @@ void SMController::setInitialRocketCoordinates(
}
}
-void SMController::moveStepperDeg(StepperList stepperId, float angle)
+ErrorMovement 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");
+ return ErrorMovement::NOT_TEST;
}
else
{
- ModuleManager::getInstance().get<Actuators>()->moveDeg(stepperId,
- angle);
+ return ModuleManager::getInstance().get<Actuators>()->moveDeg(stepperId,
+ angle);
}
}
-void SMController::moveStepperSteps(StepperList stepperId, int16_t steps)
+ErrorMovement 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");
+ return ErrorMovement::NOT_TEST;
}
else
{
- ModuleManager::getInstance().get<Actuators>()->move(stepperId, steps);
+ return ModuleManager::getInstance().get<Actuators>()->move(stepperId,
+ steps);
}
}
diff --git a/src/boards/Groundstation/Automated/SMController/SMController.h b/src/boards/Groundstation/Automated/SMController/SMController.h
index 5eb7dcd692d4a199cbd72e6d8e8a7a695fd66ce1..2b751da75b52a6771e9457683b291896e40855eb 100644
--- a/src/boards/Groundstation/Automated/SMController/SMController.h
+++ b/src/boards/Groundstation/Automated/SMController/SMController.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Federico Lolli
+ * Author: Federico Lolli, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -91,12 +91,12 @@ public:
/**
* @brief move the stepper `stepperId` of `angle` degrees
*/
- void moveStepperDeg(StepperList stepperId, float angle);
+ ErrorMovement moveStepperDeg(StepperList stepperId, float angle);
/**
* @brief move the stepper `stepperId` of `steps` steps
*/
- void moveStepperSteps(StepperList stepperId, int16_t steps);
+ ErrorMovement moveStepperSteps(StepperList stepperId, int16_t steps);
/**
* @brief Getter for follower target angles