From 3272fcda6303bf69213c5647f81d134345f6de5f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Fri, 31 May 2024 08:15:36 +0000
Subject: [PATCH] [arp-sm-ack-nack-dev] Explicit ACK/NACK checking that command
can be forwarded to steppers
Now an explicit NACK is sent in case the steppers cannot be actuated (e.g. disarmed or other reasons) or actuated in a non-expected way (limited actuation)
---
.../Automated/Actuators/Actuators.cpp | 46 +++++++++++++------
.../Automated/Actuators/Actuators.h | 26 ++++++++---
.../Automated/Follower/Follower.cpp | 22 +++++++--
src/boards/Groundstation/Automated/Hub.cpp | 18 ++++++--
.../Automated/SMController/SMController.cpp | 16 ++++---
.../Automated/SMController/SMController.h | 6 +--
6 files changed, 97 insertions(+), 37 deletions(-)
diff --git a/src/boards/Groundstation/Automated/Actuators/Actuators.cpp b/src/boards/Groundstation/Automated/Actuators/Actuators.cpp
index 3899485f7..2b69c6198 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 84243dac0..c78b9e7ab 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 cfc0c96d2..0a5276059 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 d1b3d9845..caa0dd88a 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 58676053a..b62045068 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 5eb7dcd69..2b751da75 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
--
GitLab