From 989dcf84ffdc55bd27c54be81da3c7702da26c89 Mon Sep 17 00:00:00 2001
From: Matteo Pignataro <matteo.pignataro@skywarder.eu>
Date: Sun, 11 Sep 2022 10:35:05 +0200
Subject: [PATCH] [Can] Added calibration common event [Payload] Added xbee
logging on message received
---
src/boards/Main/CanHandler/CanHandler.cpp | 9 +++
src/boards/Main/CanHandler/CanHandler.h | 1 +
src/boards/Main/Configs/CanHandlerConfig.h | 2 +
src/boards/Payload/CanHandler/CanHandler.cpp | 9 +++
src/boards/Payload/CanHandler/CanHandler.h | 2 +
src/boards/Payload/Configs/CanHandlerConfig.h | 2 +
src/boards/Payload/Radio/Radio.cpp | 77 ++++++++++++++++++-
src/boards/common/CanConfig.h | 1 +
src/entrypoints/Payload/payload-entry.cpp | 6 +-
9 files changed, 105 insertions(+), 4 deletions(-)
diff --git a/src/boards/Main/CanHandler/CanHandler.cpp b/src/boards/Main/CanHandler/CanHandler.cpp
index 6381cfe15..3bd1c352e 100644
--- a/src/boards/Main/CanHandler/CanHandler.cpp
+++ b/src/boards/Main/CanHandler/CanHandler.cpp
@@ -60,6 +60,15 @@ void CanHandler::sendDisarmEvent()
static_cast<uint8_t>(EventId::DISARM));
}
+void CanHandler::sendCalibrateEvent()
+{
+ protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL),
+ static_cast<uint8_t>(PrimaryType::EVENTS),
+ static_cast<uint8_t>(Board::MAIN),
+ static_cast<uint8_t>(Board::BROADCAST),
+ static_cast<uint8_t>(EventId::CALIBRATE));
+}
+
void CanHandler::sendCamOnEvent()
{
protocol->enqueueEvent(static_cast<uint8_t>(Priority::MEDIUM),
diff --git a/src/boards/Main/CanHandler/CanHandler.h b/src/boards/Main/CanHandler/CanHandler.h
index ddbc6763a..447a0bcdf 100644
--- a/src/boards/Main/CanHandler/CanHandler.h
+++ b/src/boards/Main/CanHandler/CanHandler.h
@@ -43,6 +43,7 @@ public:
void sendArmEvent();
void sendDisarmEvent();
+ void sendCalibrateEvent();
void sendCamOnEvent();
void sendCamOffEvent();
void sendLiftoffEvent();
diff --git a/src/boards/Main/Configs/CanHandlerConfig.h b/src/boards/Main/Configs/CanHandlerConfig.h
index aa8cc0bd2..15c618408 100644
--- a/src/boards/Main/Configs/CanHandlerConfig.h
+++ b/src/boards/Main/Configs/CanHandlerConfig.h
@@ -38,6 +38,7 @@ namespace CanHandlerConfig
static const std::map<Common::CanConfig::EventId, Common::Events> eventToEvent{
{Common::CanConfig::EventId::ARM, Common::TMTC_ARM},
{Common::CanConfig::EventId::DISARM, Common::TMTC_DISARM},
+ {Common::CanConfig::EventId::CALIBRATE, Common::TMTC_CALIBRATE},
{Common::CanConfig::EventId::CAM_ON, Common::TMTC_START_RECORDING},
{Common::CanConfig::EventId::CAM_OFF, Common::TMTC_STOP_RECORDING},
};
@@ -46,6 +47,7 @@ static const std::map<Common::Events, std::function<void(CanHandler *)>>
eventToFunction{
{Common::TMTC_ARM, &CanHandler::sendArmEvent},
{Common::TMTC_DISARM, &CanHandler::sendDisarmEvent},
+ {Common::TMTC_CALIBRATE, &CanHandler::sendCalibrateEvent},
{Common::TMTC_START_RECORDING, &CanHandler::sendCamOnEvent},
{Common::TMTC_STOP_RECORDING, &CanHandler::sendCamOffEvent},
{Common::FLIGHT_LIFTOFF, &CanHandler::sendLiftoffEvent},
diff --git a/src/boards/Payload/CanHandler/CanHandler.cpp b/src/boards/Payload/CanHandler/CanHandler.cpp
index b2fcbc47c..1d3d165b1 100644
--- a/src/boards/Payload/CanHandler/CanHandler.cpp
+++ b/src/boards/Payload/CanHandler/CanHandler.cpp
@@ -67,6 +67,15 @@ void CanHandler::sendDisarmEvent()
static_cast<uint8_t>(EventId::DISARM));
}
+void CanHandler::sendCalibrateEvent()
+{
+ protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL),
+ static_cast<uint8_t>(PrimaryType::EVENTS),
+ static_cast<uint8_t>(Board::PAYLOAD),
+ static_cast<uint8_t>(Board::BROADCAST),
+ static_cast<uint8_t>(EventId::CALIBRATE));
+}
+
void CanHandler::sendCamOnEvent()
{
protocol->enqueueEvent(static_cast<uint8_t>(Priority::MEDIUM),
diff --git a/src/boards/Payload/CanHandler/CanHandler.h b/src/boards/Payload/CanHandler/CanHandler.h
index e349ca033..8a9224d9f 100644
--- a/src/boards/Payload/CanHandler/CanHandler.h
+++ b/src/boards/Payload/CanHandler/CanHandler.h
@@ -46,6 +46,8 @@ public:
void sendDisarmEvent();
+ void sendCalibrateEvent();
+
void sendCamOnEvent();
void sendCamOffEvent();
diff --git a/src/boards/Payload/Configs/CanHandlerConfig.h b/src/boards/Payload/Configs/CanHandlerConfig.h
index 00105bffa..c0971d828 100644
--- a/src/boards/Payload/Configs/CanHandlerConfig.h
+++ b/src/boards/Payload/Configs/CanHandlerConfig.h
@@ -38,6 +38,7 @@ namespace CanHandlerConfig
static const std::map<Common::CanConfig::EventId, Common::Events> eventToEvent{
{Common::CanConfig::EventId::ARM, Common::TMTC_ARM},
{Common::CanConfig::EventId::DISARM, Common::TMTC_DISARM},
+ {Common::CanConfig::EventId::CALIBRATE, Common::TMTC_CALIBRATE},
{Common::CanConfig::EventId::CAM_ON, Common::TMTC_START_RECORDING},
{Common::CanConfig::EventId::CAM_OFF, Common::TMTC_STOP_RECORDING},
{Common::CanConfig::EventId::LIFTOFF, Common::TMTC_FORCE_LAUNCH},
@@ -48,6 +49,7 @@ static const std::map<Common::Events, std::function<void(CanHandler *)>>
eventToFunction{
{Common::TMTC_ARM, &CanHandler::sendArmEvent},
{Common::TMTC_DISARM, &CanHandler::sendDisarmEvent},
+ {Common::TMTC_CALIBRATE, &CanHandler::sendCalibrateEvent},
{Common::TMTC_START_RECORDING, &CanHandler::sendCamOnEvent},
{Common::TMTC_STOP_RECORDING, &CanHandler::sendCamOffEvent},
};
diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp
index 77f1ec8d9..5f5fe24c4 100644
--- a/src/boards/Payload/Radio/Radio.cpp
+++ b/src/boards/Payload/Radio/Radio.cpp
@@ -35,6 +35,7 @@
#include <Payload/Wing/WingController.h>
#include <common/events/Events.h>
#include <drivers/interrupt/external_interrupts.h>
+#include <radio/Xbee/APIFramesLog.h>
#include <radio/Xbee/ATCommands.h>
#include <functional>
@@ -128,7 +129,81 @@ Radio::Radio()
STATS_TM_PERIOD, STATS_TM_TASK_ID);
}
-void Radio::onXbeeFrameReceived(Boardcore::Xbee::APIFrame& frame) {}
+void Radio::onXbeeFrameReceived(Boardcore::Xbee::APIFrame& frame)
+{
+ using namespace Xbee;
+ bool logged = false;
+ switch (frame.frameType)
+ {
+ case FTYPE_AT_COMMAND:
+ {
+ ATCommandFrameLog dest;
+ logged = ATCommandFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ {
+ Logger::getInstance().log(dest);
+ }
+ break;
+ }
+ case FTYPE_AT_COMMAND_RESPONSE:
+ {
+ ATCommandResponseFrameLog dest;
+ logged = ATCommandResponseFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ {
+ Logger::getInstance().log(dest);
+ }
+ break;
+ }
+ case FTYPE_MODEM_STATUS:
+ {
+ ModemStatusFrameLog dest;
+ logged = ModemStatusFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ {
+ Logger::getInstance().log(dest);
+ }
+ break;
+ }
+ case FTYPE_TX_REQUEST:
+ {
+ TXRequestFrameLog dest;
+ logged = TXRequestFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ {
+ Logger::getInstance().log(dest);
+ }
+ break;
+ }
+ case FTYPE_TX_STATUS:
+ {
+ TXStatusFrameLog dest;
+ logged = TXStatusFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ {
+ Logger::getInstance().log(dest);
+ }
+ break;
+ }
+ case FTYPE_RX_PACKET_FRAME:
+ {
+ RXPacketFrameLog dest;
+ logged = RXPacketFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ {
+ Logger::getInstance().log(dest);
+ }
+ break;
+ }
+ }
+
+ if (!logged)
+ {
+ APIFrameLog api;
+ APIFrameLog::fromAPIFrame(frame, &api);
+ Logger::getInstance().log(api);
+ }
+}
void Radio::handleMavlinkMessage(MavDriver* driver,
const mavlink_message_t& msg)
diff --git a/src/boards/common/CanConfig.h b/src/boards/common/CanConfig.h
index 14741a5dc..1e28d68dd 100644
--- a/src/boards/common/CanConfig.h
+++ b/src/boards/common/CanConfig.h
@@ -66,6 +66,7 @@ enum class EventId : uint8_t
CAM_ON = 0,
CAM_OFF,
ERROR,
+ CALIBRATE,
ARM,
DISARM,
LIFTOFF,
diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp
index 8c0eff177..8846f8413 100644
--- a/src/entrypoints/Payload/payload-entry.cpp
+++ b/src/entrypoints/Payload/payload-entry.cpp
@@ -135,12 +135,12 @@ int main()
// Set up the wing controller
WingController::getInstance().addAlgorithm(new AutomaticWingAlgorithm(
0.1, 0.01, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO));
- WingController::getInstance().addAlgorithm(new AutomaticWingAlgorithm(
- 1, 0, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO));
+ // WingController::getInstance().addAlgorithm(new AutomaticWingAlgorithm(
+ // 1, 0, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO));
WingController::getInstance().addAlgorithm(new FileWingAlgorithm(
PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO, "/sd/servoCorta.csv"));
- WingController::getInstance().selectAlgorithm(1);
+ WingController::getInstance().selectAlgorithm(0);
// If all is correctly set up i publish the init ok
if (initResult)
--
GitLab