From 09e23e85cf69f414f62014d8e9c157a979f94fe6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu> Date: Mon, 9 Sep 2024 20:33:10 +0200 Subject: [PATCH] [Payload][Radio] Implement SET_ORIENTATION_QUAT telemetry message --- src/boards/Payload/Radio/MessageHandler.cpp | 30 +++++++++++++++++++ .../NASController/NASController.cpp | 12 ++++++++ .../NASController/NASController.h | 2 ++ 3 files changed, 44 insertions(+) diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp index a9be8eb26..63ab340e2 100644 --- a/src/boards/Payload/Radio/MessageHandler.cpp +++ b/src/boards/Payload/Radio/MessageHandler.cpp @@ -183,6 +183,36 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) return enqueueWack(msg); } + case MAVLINK_MSG_ID_SET_ORIENTATION_QUAT_TC: + { + if (parent.getModule<NASController>()->getState() != + NASControllerState::READY) + { + return enqueueNack(msg); + } + + // Scalar first quaternion, W is the first element + auto quat = Eigen::Quaternionf{ + mavlink_msg_set_orientation_quat_tc_get_quat_w(&msg), + mavlink_msg_set_orientation_quat_tc_get_quat_x(&msg), + mavlink_msg_set_orientation_quat_tc_get_quat_y(&msg), + mavlink_msg_set_orientation_quat_tc_get_quat_z(&msg), + }; + + parent.getModule<NASController>()->setOrientation( + quat.normalized()); + + float qNorm = quat.norm(); + if (std::abs(qNorm - 1) > 0.001) + { + return enqueueWack(msg); + } + else + { + return enqueueAck(msg); + } + } + case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC: { float altitude = diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp index 93f1c5e5a..0b1569529 100644 --- a/src/boards/Payload/StateMachines/NASController/NASController.cpp +++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp @@ -101,6 +101,18 @@ ReferenceValues NASController::getReferenceValues() NASControllerState NASController::getState() { return state; } +void NASController::setOrientation(const Eigen::Quaternionf& quat) +{ + Lock<FastMutex> lock{nasMutex}; + + auto x = nas.getX(); + x(NAS::IDX_QUAT + 0) = quat.x(); + x(NAS::IDX_QUAT + 1) = quat.y(); + x(NAS::IDX_QUAT + 2) = quat.z(); + x(NAS::IDX_QUAT + 3) = quat.w(); + nas.setX(x); +} + void NASController::Init(const Event& event) { switch (event) diff --git a/src/boards/Payload/StateMachines/NASController/NASController.h b/src/boards/Payload/StateMachines/NASController/NASController.h index 8d3a7e196..4ff4eea6a 100644 --- a/src/boards/Payload/StateMachines/NASController/NASController.h +++ b/src/boards/Payload/StateMachines/NASController/NASController.h @@ -60,6 +60,8 @@ public: NASControllerState getState(); + void setOrientation(const Eigen::Quaternionf& orientation); + private: void calibrate(); -- GitLab