From 921624d77a6cd29c3b28dabf5d46a2325ee9b7b2 Mon Sep 17 00:00:00 2001
From: Luca Conterio <luca.conterio@skywarder.eu>
Date: Sat, 27 Nov 2021 18:33:47 +0100
Subject: [PATCH] [Payload] Added PinHandler for nosecone detach pin
---
cmake/dependencies.cmake | 1 +
src/boards/Payload/PayloadBoard.h | 12 +--
src/boards/Payload/PinHandler/PinHandler.cpp | 83 ++++++++++++++++++
src/boards/Payload/PinHandler/PinHandler.h | 84 +++++++++++++++++++
.../Payload/PinHandler/PinHandlerData.h | 66 +++++++++++++++
src/boards/Payload/configs/PinHandlerConfig.h | 60 +++++++++++++
6 files changed, 301 insertions(+), 5 deletions(-)
create mode 100644 src/boards/Payload/PinHandler/PinHandler.cpp
create mode 100644 src/boards/Payload/PinHandler/PinHandler.h
create mode 100644 src/boards/Payload/PinHandler/PinHandlerData.h
create mode 100644 src/boards/Payload/configs/PinHandlerConfig.h
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 9b75f9864..d067614da 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -64,6 +64,7 @@ set(PAYLOAD_SOURCES
src/boards/Payload/WingControl/WingServo.cpp
src/boards/Payload/Main/Sensors.cpp
src/boards/Payload/Main/Radio.cpp
+ src/boards/Payload/PinHandler/PinHandler.cpp
)
set(ADA_SOURCES
src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp
diff --git a/src/boards/Payload/PayloadBoard.h b/src/boards/Payload/PayloadBoard.h
index 7b32479c7..c01728f44 100644
--- a/src/boards/Payload/PayloadBoard.h
+++ b/src/boards/Payload/PayloadBoard.h
@@ -30,8 +30,8 @@
#include <Payload/Main/Bus.h>
#include <Payload/Main/Radio.h>
#include <Payload/Main/Sensors.h>
-//#include <Main/StateMachines.h>
-//#include <PinHandler/PinHandler.h>
+//#include <Payload/Main/StateMachines.h>
+#include <Payload/PinHandler/PinHandler.h>
#include <System/StackLogger.h>
#include <System/TaskID.h>
#include <events/EventBroker.h>
@@ -76,7 +76,7 @@ public:
Radio* radio;
Actuators* actuators;
- //PinHandler* pin_handler;
+ PinHandler* pin_handler;
TaskScheduler* scheduler;
@@ -104,13 +104,13 @@ public:
{
LOG_ERR(log, "Error starting state machines");
status.setError(&PayloadStatus::state_machines);
- }
+ }*/
if (!pin_handler->start())
{
LOG_ERR(log, "Error starting PinObserver");
status.setError(&PayloadStatus::pin_obs);
- }*/
+ }
#ifdef DEBUG
injector->start();
@@ -176,6 +176,8 @@ private:
sensors = new Sensors(*bus->spi1, scheduler);
actuators = new Actuators();
+ pin_handler = new PinHandler();
+
#ifdef DEBUG
injector = new EventInjector();
#endif
diff --git a/src/boards/Payload/PinHandler/PinHandler.cpp b/src/boards/Payload/PinHandler/PinHandler.cpp
new file mode 100644
index 000000000..165b0a87a
--- /dev/null
+++ b/src/boards/Payload/PinHandler/PinHandler.cpp
@@ -0,0 +1,83 @@
+/* Copyright (c) 2019-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+//#include <LoggerService/LoggerService.h>
+#include <Payload/PinHandler/PinHandler.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/EventBroker.h>
+#include <events/Events.h>
+//#include <Payload/PayloadBoard.h>
+
+#include <functional>
+
+using std::bind;
+using namespace Boardcore;
+
+namespace PayloadBoard
+{
+
+PinHandler::PinHandler()
+ : pin_obs(PIN_POLL_INTERVAL) //, logger(LoggerService::getInstance())
+{
+ // Used for _1, _2. See std::bind cpp reference
+ using namespace std::placeholders;
+
+ // Noseconse pin callbacks registration
+ PinObserver::OnTransitionCallback nc_transition_cb =
+ bind(&PinHandler::onNCPinTransition, this, _1, _2);
+
+ PinObserver::OnStateChangeCallback nc_statechange_cb =
+ bind(&PinHandler::onNCPinStateChange, this, _1, _2, _3);
+
+ pin_obs.observePin(nosecone_pin.getPort(), nosecone_pin.getNumber(),
+ TRIGGER_NC_DETACH_PIN, nc_transition_cb,
+ THRESHOLD_NC_DETACH_PIN, nc_statechange_cb);
+}
+
+void PinHandler::onNCPinTransition(unsigned int p, unsigned char n)
+{
+ UNUSED(p);
+ UNUSED(n);
+ sEventBroker->post(Event{EV_NC_DETACHED}, TOPIC_FLIGHT_EVENTS);
+
+ LOG_INFO(log, "Nosecone detached!");
+
+ status_pin_nosecone.last_detection_time = TimestampTimer::getTimestamp();
+ //logger->log(status_pin_nosecone);
+}
+
+void PinHandler::onNCPinStateChange(unsigned int p, unsigned char n, int state)
+{
+ UNUSED(p);
+ UNUSED(n);
+
+ status_pin_nosecone.state = (uint8_t)state;
+ status_pin_nosecone.last_state_change = TimestampTimer::getTimestamp();
+ status_pin_nosecone.num_state_changes += 1;
+
+ LOG_INFO(log, "Nosecone pin state change at time {}: new state = {}",
+ status_pin_nosecone.last_state_change, status_pin_nosecone.state);
+
+ //logger->log(status_pin_nosecone);
+}
+
+} // namespace PayloadBoard
\ No newline at end of file
diff --git a/src/boards/Payload/PinHandler/PinHandler.h b/src/boards/Payload/PinHandler/PinHandler.h
new file mode 100644
index 000000000..fdc081e0b
--- /dev/null
+++ b/src/boards/Payload/PinHandler/PinHandler.h
@@ -0,0 +1,84 @@
+/* Copyright (c) 2019-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Payload/PinHandler/PinHandlerData.h>
+#include <Payload/configs/PinHandlerConfig.h>
+#include <diagnostic/PrintLogger.h>
+#include <utils/PinObserver.h>
+
+using namespace Boardcore;
+
+namespace PayloadBoard
+{
+
+/**
+ * @brief Forward dec.
+ */
+//class LoggerService;
+
+/**
+ * @brief This class contains the handlers for both the launch pin (umbilical)
+ * and the nosecone detachment pin.
+ *
+ * It uses boardcore's PinObserver to bind these functions to the GPIO pins.
+ * The handlers post an event on the EventBroker.
+ */
+class PinHandler
+{
+public:
+ PinHandler();
+
+ /**
+ * @brief Starts the pin observer.
+ *
+ */
+ bool start() { return pin_obs.start(); }
+
+ /**
+ * @brief Stops the pin observer.
+ *
+ */
+ void stop() { pin_obs.stop(); }
+
+ /**
+ * @brief Function called by the pinobserver when a nosecone pin detachment
+ * is detected.
+ *
+ * @param p
+ * @param n
+ */
+ void onNCPinTransition(unsigned int p, unsigned char n);
+
+ void onNCPinStateChange(unsigned int p, unsigned char n, int state);
+
+private:
+ PinStatus status_pin_nosecone{ObservedPin::NOSECONE};
+
+ PinObserver pin_obs;
+
+ //LoggerService* logger;
+ PrintLogger log = Logging::getLogger("deathstack.pinhandler");
+};
+
+} // namespace PayloadBoard
\ No newline at end of file
diff --git a/src/boards/Payload/PinHandler/PinHandlerData.h b/src/boards/Payload/PinHandler/PinHandlerData.h
new file mode 100644
index 000000000..e3c0328df
--- /dev/null
+++ b/src/boards/Payload/PinHandler/PinHandlerData.h
@@ -0,0 +1,66 @@
+/* Copyright (c) 2019-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <ostream>
+
+namespace PayloadBoard
+{
+
+enum class ObservedPin : uint8_t
+{
+ NOSECONE = 0
+};
+
+/**
+ * @brief Struct represeting the status of an observed pin.
+ *
+ */
+struct PinStatus
+{
+ ObservedPin pin;
+
+ uint64_t last_state_change = 0; // Last time the pin changed state
+ uint8_t state = 0; // Current state of the pin
+ unsigned int num_state_changes = 0;
+
+ uint64_t last_detection_time = 0; // When a transition is detected
+
+ PinStatus(){};
+ PinStatus(ObservedPin pin) : pin(pin) {}
+
+ static std::string header()
+ {
+ return "pin,last_state_change,state,num_state_changes,last_detection_"
+ "time\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << (int)pin << "," << last_state_change << "," << (int)state << ","
+ << num_state_changes << "," << last_detection_time << "\n";
+ }
+};
+
+} // namespace PayloadBoard
diff --git a/src/boards/Payload/configs/PinHandlerConfig.h b/src/boards/Payload/configs/PinHandlerConfig.h
new file mode 100644
index 000000000..76588cbbb
--- /dev/null
+++ b/src/boards/Payload/configs/PinHandlerConfig.h
@@ -0,0 +1,60 @@
+/* Copyright (c) 2019-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <interfaces-impl/hwmapping.h>
+#include <miosix.h>
+#include <utils/PinObserver.h>
+
+using namespace Boardcore;
+
+namespace PayloadBoard
+{
+
+static const unsigned int PIN_POLL_INTERVAL = 10; // ms
+
+// // Launch pin config
+// static const GpioPin launch_pin(miosix::inputs::lp_dtch::getPin());
+// static const PinObserver::Transition TRIGGER_LAUNCH_PIN =
+// PinObserver::Transition::FALLING_EDGE;
+// // How many consecutive times the launch pin should be detected as detached
+// // before triggering a launch event.
+// static const unsigned int THRESHOLD_LAUNCH_PIN = 10;
+
+// Nosecone detach pin config
+static const GpioPin nosecone_pin(miosix::inputs::nc_dtch::getPin());
+static const PinObserver::Transition TRIGGER_NC_DETACH_PIN =
+ PinObserver::Transition::FALLING_EDGE;
+// How many consecutive times the nosecone pin should be detected as detached
+// before triggering a nosecone detach event.
+static const unsigned int THRESHOLD_NC_DETACH_PIN = 10;
+
+// // Dpl servo actuation pin config
+// static const GpioPin dpl_servo_pin(miosix::inputs::expulsion_in::getPin());
+// static const PinObserver::Transition TRIGGER_DPL_SERVO_PIN =
+// PinObserver::Transition::RISING_EDGE;
+// // How many consecutive times the deployment servo pin should be detected as
+// // detached before triggering a nosecone detach event.
+// static const unsigned int THRESHOLD_DPL_SERVO_PIN = 10;
+
+} // namespace PayloadBoard
\ No newline at end of file
--
GitLab