diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp index 52295a00f836775727a3dd8a8117b65635573f2f..6cef566dc4859f385e7fefd29ba97ee13661e003 100644 --- a/src/boards/Payload/Radio/MessageHandler.cpp +++ b/src/boards/Payload/Radio/MessageHandler.cpp @@ -35,6 +35,7 @@ #include <common/Topics.h> #include <diagnostic/CpuMeter/CpuMeter.h> #include <events/EventBroker.h> +#include <interfaces-impl/hwmapping.h> #include "Radio.h" @@ -190,7 +191,7 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) parent.getModule<AltitudeTrigger>()->setDeploymentAltitude( altitude); - if (altitude < 0 || altitude > 3000) + if (altitude < 100 || altitude > 3000) { return enqueueWack(msg); } @@ -200,6 +201,45 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) } } + case MAVLINK_MSG_ID_SET_TARGET_COORDINATES_TC: + { + float latitude = + mavlink_msg_set_target_coordinates_tc_get_latitude(&msg); + float longitude = + mavlink_msg_set_target_coordinates_tc_get_longitude(&msg); + + bool targetSet = + parent.getModule<WingController>()->setTargetCoordinates( + latitude, longitude); + + if (targetSet) + { + return enqueueAck(msg); + } + else + { + return enqueueNack(msg); + } + } + + case MAVLINK_MSG_ID_SET_ALGORITHM_TC: + { + uint8_t index = + mavlink_msg_set_algorithm_tc_get_algorithm_number(&msg); + + bool algorithmSet = + parent.getModule<WingController>()->selectAlgorithm(index); + + if (algorithmSet) + { + return enqueueAck(msg); + } + else + { + return enqueueNack(msg); + } + } + case MAVLINK_MSG_ID_RAW_EVENT_TC: { uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg); @@ -534,11 +574,13 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) mavlink_payload_stats_tm_t tm; auto* nas = parent.getModule<NASController>(); + auto* wnc = parent.getModule<WingController>(); auto* pinHandler = parent.getModule<PinHandler>(); auto& logger = Logger::getInstance(); auto stats = parent.getModule<FlightStatsRecorder>()->getStats(); auto ref = nas->getReferenceValues(); + auto emPoints = wnc->getEarlyManeuverPoints(); auto cpuStats = CpuMeter::getCpuStats(); auto loggerStats = logger.getStats(); @@ -567,12 +609,12 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) tm.apogee_max_acc = stats.apogeeMaxAcc; // Wing stats - tm.wing_emc_n = -1.0f; // TODO - tm.wing_emc_e = -1.0f; // TODO - tm.wing_m1_n = -1.0f; // TODO - tm.wing_m1_e = -1.0f; // TODO - tm.wing_m2_n = -1.0f; // TODO - tm.wing_m2_e = -1.0f; // TODO + tm.wing_emc_n = emPoints.emcN; + tm.wing_emc_e = emPoints.emcE; + tm.wing_m1_n = emPoints.m1N; + tm.wing_m1_e = emPoints.m1E; + tm.wing_m2_n = emPoints.m2N; + tm.wing_m2_e = emPoints.m2E; // Deployment stats tm.dpl_ts = stats.dplTs; @@ -596,14 +638,14 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) tm.log_number = loggerStats.logNumber; tm.nas_state = static_cast<uint8_t>(nas->getState()); - tm.wes_state = 255; // TODO + tm.wes_state = static_cast<uint8_t>(wnc->getState()); // Pins tm.pin_launch = pinHandler->getPinData(PinList::RAMP_DETACH_PIN).lastState; tm.pin_nosecone = pinHandler->getPinData(PinList::NOSECONE_DETACH_PIN).lastState; - tm.cutter_presence = 255; // TODO + tm.cutter_presence = miosix::sense::cutterSense::value(); auto canStatus = parent.getModule<CanHandler>()->getCanStatus(); tm.main_board_state = canStatus.mainState; diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp index 13d02817f39e78d06e9dbb718adb09688bad3064..156560b12be176499dee468d1549199d72b85210 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.cpp +++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp @@ -324,7 +324,7 @@ bool WingController::isStarted() { return started; } WingControllerState WingController::getState() { return state; } -bool WingController::setTargetPosition(Eigen::Vector2f targetPositionGEO) +bool WingController::setTargetCoordinates(float latitude, float longitude) { // Allow changing the target position in the IDLE state only if (state != WingControllerState::IDLE) @@ -332,11 +332,22 @@ bool WingController::setTargetPosition(Eigen::Vector2f targetPositionGEO) return false; } - this->targetPositionGEO = targetPositionGEO; + this->targetPositionGEO = Eigen::Vector2f{latitude, longitude}; + + // Log early maneuver points to highlight any discrepancies if any + auto earlyManeuverPoints = getEarlyManeuverPoints(); auto data = WingTargetPositionData{ - .targetLat = targetPositionGEO[0], .targetLon = targetPositionGEO[1], - // TODO: populate early maneuver points + .targetLat = targetPositionGEO[0], + .targetLon = targetPositionGEO[1], + .targetN = earlyManeuverPoints.targetN, + .targetE = earlyManeuverPoints.targetE, + .emcN = earlyManeuverPoints.emcN, + .emcE = earlyManeuverPoints.emcE, + .m1N = earlyManeuverPoints.m1N, + .m1E = earlyManeuverPoints.m1E, + .m2N = earlyManeuverPoints.m2N, + .m2E = earlyManeuverPoints.m2E, }; Logger::getInstance().log(data); @@ -375,6 +386,11 @@ bool WingController::selectAlgorithm(uint8_t index) } } +EarlyManeuversPoints WingController::getEarlyManeuverPoints() +{ + return emGuidance.getPoints(); +} + void WingController::loadAlgorithms() { // Early Maneuver Guidance Automatic Algorithm diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h index 66874623bff274daff39d7381e80922d22bda4ff..b1a5d3187f5e273fbe446b74bba2b76613e25d93 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.h +++ b/src/boards/Payload/StateMachines/WingController/WingController.h @@ -104,10 +104,9 @@ public: WingControllerState getState(); /** - * @brief Sets the target position. - * @note The provided position must be using geodetic coordinates. + * @brief Sets the target coordinates. */ - bool setTargetPosition(Eigen::Vector2f targetPositionGEO); + bool setTargetCoordinates(float latitude, float longitude); /** * @brief Changes the selected algorithm. @@ -115,6 +114,11 @@ public: */ bool selectAlgorithm(uint8_t index); + /** + * @brief Returns the currently set early maneuver points. + */ + EarlyManeuversPoints getEarlyManeuverPoints(); + /** * @brief This is a forward method to the early maneuvers guidance algorithm * to calculate the yaw angle of the parafoil knowing the current position diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp index 698f5f6cb04493188ee52a8b65b7ba5df712e028..e4da61dd669860cf59608b4fa499abf3f9976832 100644 --- a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp +++ b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp @@ -147,4 +147,18 @@ void EarlyManeuversGuidanceAlgorithm::setPoints(Eigen::Vector2f targetNED, this->M2 = M2; } +EarlyManeuversPoints EarlyManeuversGuidanceAlgorithm::getPoints() +{ + return EarlyManeuversPoints{ + .targetN = targetNED[0], + .targetE = targetNED[1], + .emcN = EMC[0], + .emcE = EMC[1], + .m1N = M1[0], + .m1E = M1[1], + .m2N = M2[0], + .m2E = M2[1], + }; +} + } // namespace Payload diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h index 30a432504dbf6404c0219f62d88c4490c8d0f4b0..c52f76189a7bd9f7b195f966e157a3d1af0f3a39 100644 --- a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h +++ b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h @@ -29,6 +29,18 @@ namespace Payload { +struct EarlyManeuversPoints +{ + float targetN; + float targetE; + float emcN; + float emcE; + float m1N; + float m1E; + float m2N; + float m2E; +}; + /** * This class is the implementation of the Simple Closed Loop guidance. * It calculates the yaw between the current position and the target position by @@ -75,6 +87,11 @@ public: void setPoints(Eigen::Vector2f targetNED, Eigen::Vector2f EMC, Eigen::Vector2f M1, Eigen::Vector2f M2); + /** + * @brief Get Early Maneuvers points. + */ + EarlyManeuversPoints getPoints(); + private: /** @brief Updates the class target *