From e39c8377a0d8d6cc65d1a3e43137279129e358b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu> Date: Mon, 9 Sep 2024 19:12:31 +0200 Subject: [PATCH] [Payload][Radio] Update telemetry for updated mavlink --- src/boards/Payload/Radio/MessageHandler.cpp | 59 ++++++++++--------- .../WingController/WingController.cpp | 52 ++++++++++------ .../WingController/WingController.h | 32 ++++++++-- .../EarlyManeuverGuidanceAlgorithm.cpp | 17 ++++++ .../EarlyManeuversGuidanceAlgorithm.h | 8 ++- 5 files changed, 117 insertions(+), 51 deletions(-) diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp index 6cef566dc..a9be8eb26 100644 --- a/src/boards/Payload/Radio/MessageHandler.cpp +++ b/src/boards/Payload/Radio/MessageHandler.cpp @@ -502,19 +502,21 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) auto* wes = parent.getModule<WindEstimation>(); auto* fmm = parent.getModule<FlightModeManager>(); - auto imu = sensors->getLSM6DSRXLastSample(); - auto mag = sensors->getLIS2MDLLastSample(); - auto gps = sensors->getUBXGPSLastSample(); - auto pressDigi = sensors->getLPS28DFWLastSample(); - auto pressStatic = sensors->getStaticPressureLastSample(); - auto nasState = nas->getNasState(); - auto wind = wes->getWindEstimationScheme(); - - tm.timestamp = TimestampTimer::getTimestamp(); - tm.pressure_digi = pressDigi.pressure; - tm.pressure_static = pressStatic.pressure; - tm.airspeed_pitot = -1.0f; // TODO - tm.altitude_agl = -nasState.d; + auto imu = sensors->getLSM6DSRXLastSample(); + auto mag = sensors->getLIS2MDLLastSample(); + auto gps = sensors->getUBXGPSLastSample(); + auto pressDigi = sensors->getLPS28DFWLastSample(); + auto pressStatic = sensors->getStaticPressureLastSample(); + auto pressDynamic = sensors->getDynamicPressureLastSample(); + auto nasState = nas->getNasState(); + auto wind = wes->getWindEstimationScheme(); + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.pressure_digi = pressDigi.pressure; + tm.pressure_static = pressStatic.pressure; + tm.pressure_dynamic = pressDynamic.pressure; + tm.airspeed_pitot = -1.0f; // TODO + tm.altitude_agl = -nasState.d; // Sensors tm.acc_x = imu.accelerationX; @@ -551,8 +553,8 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) tm.nas_bias_x = nasState.bx; tm.nas_bias_y = nasState.by; tm.nas_bias_z = nasState.bz; - tm.wes_n = wind[0]; - tm.wes_e = wind[1]; + tm.wes_n = wind.x(); + tm.wes_e = wind.y(); tm.battery_voltage = sensors->getBatteryVoltage().batVoltage; tm.cam_battery_voltage = sensors->getCamBatteryVoltage().batVoltage; @@ -578,16 +580,20 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) 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(); + auto stats = parent.getModule<FlightStatsRecorder>()->getStats(); + auto ref = nas->getReferenceValues(); + auto wingTarget = wnc->getTargetCoordinates(); + auto wingActiveTarget = wnc->getActiveTarget(); + auto wingAlgorithm = wnc->getSelectedAlgorithm(); + auto cpuStats = CpuMeter::getCpuStats(); + auto loggerStats = logger.getStats(); // Log CPU stats and reset them CpuMeter::resetCpuStats(); logger.log(cpuStats); + tm.timestamp = TimestampTimer::getTimestamp(); + // Liftoff stats tm.liftoff_ts = stats.liftoffTs; tm.liftoff_max_acc_ts = stats.liftoffMaxAccTs; @@ -609,12 +615,11 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) tm.apogee_max_acc = stats.apogeeMaxAcc; // Wing stats - 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; + tm.wing_target_lat = wingTarget.x(); + tm.wing_target_lon = wingTarget.y(); + tm.wing_active_target_n = wingActiveTarget.x(); + tm.wing_active_target_e = wingActiveTarget.y(); + tm.wing_algorithm = wingAlgorithm; // Deployment stats tm.dpl_ts = stats.dplTs; @@ -638,7 +643,7 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) tm.log_number = loggerStats.logNumber; tm.nas_state = static_cast<uint8_t>(nas->getState()); - tm.wes_state = static_cast<uint8_t>(wnc->getState()); + tm.wnc_state = static_cast<uint8_t>(wnc->getState()); // Pins tm.pin_launch = diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp index 156560b12..3bd2144a6 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.cpp +++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp @@ -324,6 +324,11 @@ bool WingController::isStarted() { return started; } WingControllerState WingController::getState() { return state; } +Eigen::Vector2f WingController::getTargetCoordinates() +{ + return targetPositionGEO.load(); +} + bool WingController::setTargetCoordinates(float latitude, float longitude) { // Allow changing the target position in the IDLE state only @@ -332,14 +337,14 @@ bool WingController::setTargetCoordinates(float latitude, float longitude) return false; } - this->targetPositionGEO = Eigen::Vector2f{latitude, longitude}; + targetPositionGEO = Coordinates{latitude, longitude}; // Log early maneuver points to highlight any discrepancies if any auto earlyManeuverPoints = getEarlyManeuverPoints(); auto data = WingTargetPositionData{ - .targetLat = targetPositionGEO[0], - .targetLon = targetPositionGEO[1], + .targetLat = latitude, + .targetLon = longitude, .targetN = earlyManeuverPoints.targetN, .targetE = earlyManeuverPoints.targetE, .emcN = earlyManeuverPoints.emcN, @@ -354,6 +359,11 @@ bool WingController::setTargetCoordinates(float latitude, float longitude) return true; } +uint8_t WingController::getSelectedAlgorithm() +{ + return static_cast<uint8_t>(selectedAlgorithm.load()); +} + bool WingController::selectAlgorithm(uint8_t index) { // Allow changing the algorithm in the IDLE state only @@ -391,6 +401,11 @@ EarlyManeuversPoints WingController::getEarlyManeuverPoints() return emGuidance.getPoints(); } +Eigen::Vector2f WingController::getActiveTarget() +{ + return emGuidance.getActiveTarget(); +} + void WingController::loadAlgorithms() { // Early Maneuver Guidance Automatic Algorithm @@ -505,17 +520,18 @@ void WingController::updateEarlyManeuverPoints() { using namespace Eigen; - auto nas = getModule<NASController>(); - auto nasState = nas->getNasState(); - auto nasRef = nas->getReferenceValues(); + auto nas = getModule<NASController>(); + auto nasState = nas->getNasState(); + auto nasRef = nas->getReferenceValues(); + auto targetGEO = targetPositionGEO.load(); Vector2f currentPositionNED = {nasState.n, nasState.e}; Vector2f targetNED = Aeroutils::geodetic2NED( - targetPositionGEO, {nasRef.refLatitude, nasRef.refLongitude}); + targetGEO, {nasRef.refLatitude, nasRef.refLongitude}); Vector2f targetOffsetNED = targetNED - currentPositionNED; Vector2f normPoint = targetOffsetNED / targetOffsetNED.norm(); - float psi0 = atan2(normPoint[1], normPoint[0]); + float psi0 = atan2(normPoint.y(), normPoint.x()); float distFromCenterline = 20; // the distance that the M1 and M2 points // must have from the center line @@ -545,16 +561,16 @@ void WingController::updateEarlyManeuverPoints() // Log the updated points auto data = WingTargetPositionData{ - .targetLat = targetPositionGEO[0], - .targetLon = targetPositionGEO[1], - .targetN = targetNED[0], - .targetE = targetNED[1], - .emcN = emcPosition[0], - .emcE = emcPosition[1], - .m1N = m1Position[0], - .m1E = m1Position[1], - .m2N = m2Position[0], - .m2E = m2Position[1], + .targetLat = targetGEO.latitude, + .targetLon = targetGEO.longitude, + .targetN = targetNED.x(), + .targetE = targetNED.y(), + .emcN = emcPosition.x(), + .emcE = emcPosition.y(), + .m1N = m1Position.x(), + .m1E = m1Position.y(), + .m2N = m2Position.x(), + .m2E = m2Position.y(), }; Logger::getInstance().log(data); } diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h index b1a5d3187..4c3ada8ed 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.h +++ b/src/boards/Payload/StateMachines/WingController/WingController.h @@ -103,11 +103,22 @@ public: WingControllerState getState(); + /** + * @brief Returns the target coordinates. + * @return The GEO coordinates of the active target. + */ + Eigen::Vector2f getTargetCoordinates(); + /** * @brief Sets the target coordinates. */ bool setTargetCoordinates(float latitude, float longitude); + /** + * @brief Returns the selected algorithm. + */ + uint8_t getSelectedAlgorithm(); + /** * @brief Changes the selected algorithm. * @return Whether the provided index selected a valid algorithm. @@ -119,6 +130,11 @@ public: */ EarlyManeuversPoints getEarlyManeuverPoints(); + /** + * @brief Returns the early maneuver active target. + */ + Eigen::Vector2f getActiveTarget(); + /** * @brief This is a forward method to the early maneuvers guidance algorithm * to calculate the yaw angle of the parafoil knowing the current position @@ -193,12 +209,18 @@ private: void updateState(WingControllerState newState); - Eigen::Vector2f targetPositionGEO{Config::Wing::Default::TARGET_LAT, - Config::Wing::Default::TARGET_LON}; + struct Coordinates + { + float latitude; + float longitude; + + operator Eigen::Vector2f() const { return {latitude, longitude}; } + }; - std::atomic<bool> targetPositionDirty{ - false}; ///< Whether the target position was changed and EM points need - ///< to be updated + std::atomic<Coordinates> targetPositionGEO{Coordinates{ + .latitude = Config::Wing::Default::TARGET_LAT, + .longitude = Config::Wing::Default::TARGET_LON, + }}; std::atomic<Config::Wing::AlgorithmId> selectedAlgorithm{ Config::Wing::Default::ALGORITHM}; diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp index e4da61dd6..aed38bf29 100644 --- a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp +++ b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp @@ -161,4 +161,21 @@ EarlyManeuversPoints EarlyManeuversGuidanceAlgorithm::getPoints() }; } +Eigen::Vector2f EarlyManeuversGuidanceAlgorithm::getActiveTarget() +{ + switch (activeTarget) + { + case Target::EMC: + return EMC; + case Target::M1: + return M1; + case Target::M2: + return M2; + case Target::FINAL: + return targetNED; + default: + return {0, 0}; + } +} + } // namespace Payload diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h index c52f76189..58ef53d3f 100644 --- a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h +++ b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h @@ -92,6 +92,12 @@ public: */ EarlyManeuversPoints getPoints(); + /** + * @brief Get the active target. + * @return The NED coordinates of the active target. + */ + Eigen::Vector2f getActiveTarget(); + private: /** @brief Updates the class target * @@ -110,7 +116,7 @@ private: }; // Point we are currently poinying to - Target activeTarget; + std::atomic<Target> activeTarget; // Eigen::Vector2f targetNED; // NED -- GitLab