From 1aa3f6f1dc2d20eff1dd894f4498fd69ff3d49f0 Mon Sep 17 00:00:00 2001 From: Alireza Ghaderi Date: Mon, 6 Nov 2023 04:16:04 +0000 Subject: [PATCH] organizing --- ConnectionManager.cpp | 3 ++ DataRefManager.cpp | 71 +++++++++++++++++++++++++++++++++++++++++++ DataRefManager.h | 9 +++++- MAVLinkManager.cpp | 58 ++++++++++++++++++++++++++++------- 4 files changed, 129 insertions(+), 12 deletions(-) diff --git a/ConnectionManager.cpp b/ConnectionManager.cpp index e1c41f2..18b26cf 100644 --- a/ConnectionManager.cpp +++ b/ConnectionManager.cpp @@ -100,6 +100,9 @@ void ConnectionManager::acceptConnection() { XPLMDebugString("px4xplane: Connected to SITL successfully.\n"); connected = true; + DataRefManager::initializeMagneticField(); + XPLMDebugString("px4xplane: Init Magnetic Done.\n"); + status = "Connected"; setLastMessage("Connected to SITL successfully."); diff --git a/DataRefManager.cpp b/DataRefManager.cpp index de7fa71..40adb6c 100644 --- a/DataRefManager.cpp +++ b/DataRefManager.cpp @@ -69,6 +69,77 @@ std::vector DataRefManager::dataRefs = { {"RC Information", "Rudder", "sim/joystick/yoke_heading_ratio", "", 1, DRT_FLOAT, std::function(getFloat)}, }; +/** + * @brief Static variable representing the Earth's magnetic field in NED (North-East-Down) coordinates. + * Initialized to zero and updated based on the aircraft's location. + */ +Eigen::Vector3f DataRefManager::earthMagneticFieldNED = Eigen::Vector3f::Zero(); + + + +/** + * @brief Converts a magnetic field vector from NED to body frame. + * + * This function uses the provided roll, pitch, and yaw angles to rotate the magnetic field vector + * from the NED frame to the body frame of the aircraft. + * + * @param nedVector Magnetic field vector in NED coordinates. + * @param roll Roll angle in radians (rotation about the X-axis). + * @param pitch Pitch angle in radians (rotation about the Y-axis). + * @param yaw Yaw angle in radians with respect to Magnetic North (rotation about the Z-axis). + * @return Eigen::Vector3f The magnetic field vector in the body frame, in Gauss. + */ +Eigen::Vector3f DataRefManager::convertNEDToBody(const Eigen::Vector3f& nedVector, float roll, float pitch, float yaw) { + Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX()); + Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf yawAngle(-yaw, Eigen::Vector3f::UnitZ()); + Eigen::Matrix3f rotationMatrix = (rollAngle * pitchAngle * yawAngle).matrix(); + + Eigen::Vector3f bodyVector = rotationMatrix * nedVector; + bodyVector *= 0.00001; // Convert from nT to Gauss + + return bodyVector; +} + + +/** + * @brief Initializes the Earth's magnetic field in NED based on the aircraft's initial location. + * + * This function should be called when the plugin or simulation starts to set the initial value + * of the Earth's magnetic field in NED. + */ +void DataRefManager::initializeMagneticField() { + float initialLatitude = getFloat("sim/flightmodel/position/latitude"); + float initialLongitude = getFloat("sim/flightmodel/position/longitude"); + float initialAltitude = getFloat("sim/flightmodel/position/elevation"); + + updateEarthMagneticFieldNED(initialLatitude, initialLongitude, initialAltitude); +} + + +/** + * @brief Updates the Earth's magnetic field in NED based on the provided geodetic coordinates. + * + * This function calculates the Earth's magnetic field in NED using the World Magnetic Model (WMM2020) + * and the provided latitude, longitude, and altitude. + * + * @param lat Latitude in degrees. + * @param lon Longitude in degrees. + * @param alt Altitude in meters above sea level. + * @return Eigen::Vector3f The updated magnetic field vector in NED coordinates. + */ +Eigen::Vector3f DataRefManager::updateEarthMagneticFieldNED(float lat, float lon, float alt) { + float latRad = lat * M_PI / 180.0; + float lonRad = lon * M_PI / 180.0; + + geomag::Vector position = geomag::geodetic2ecef(latRad, lonRad, alt); + geomag::Vector mag_field = geomag::GeoMag(2022.5, position, geomag::WMM2020); + geomag::Elements nedElements = geomag::magField2Elements(mag_field, latRad, lonRad); + + earthMagneticFieldNED = Eigen::Vector3f(nedElements.north, nedElements.east, nedElements.down); + return earthMagneticFieldNED; +} + void DataRefManager::drawDataRefs(XPLMWindowID in_window_id, int l, int t, float col_white[],int lineOffset) { diff --git a/DataRefManager.h b/DataRefManager.h index 9009a8c..4a1f90e 100644 --- a/DataRefManager.h +++ b/DataRefManager.h @@ -9,7 +9,9 @@ #include #include #include - +#include +#include +#include enum DataRefType { DRT_FLOAT, @@ -35,6 +37,8 @@ class DataRefManager { public: static constexpr float g_earth = 9.81f; static std::vector dataRefs; + static Eigen::Vector3f earthMagneticFieldNED; + static void drawDataRefs(XPLMWindowID in_window_id, int l, int t, float col_white[], int lineOffset); static std::vector getFloatArray(const char* dataRefName); @@ -49,6 +53,9 @@ class DataRefManager { static void enableOverride(); static void disableOverride(); static int drawActualThrottle(XPLMWindowID in_window_id, int l, int t, float col_white[], int lineOffset); + static Eigen::Vector3f updateEarthMagneticFieldNED(float lat, float lon, float alt); + static Eigen::Vector3f convertNEDToBody(const Eigen::Vector3f& nedVector, float roll, float pitch, float yaw); + static void initializeMagneticField(); }; diff --git a/MAVLinkManager.cpp b/MAVLinkManager.cpp index 930a8b7..d286d13 100644 --- a/MAVLinkManager.cpp +++ b/MAVLinkManager.cpp @@ -394,19 +394,43 @@ void MAVLinkManager::processHILActuatorControlsMessage(const mavlink_message_t& } - +/** + * @brief Sets the acceleration data in the HIL_SENSOR message. + * + * This function retrieves the aircraft's current acceleration forces from the simulation, + * multiplies them by the gravitational constant, and sets them in the provided HIL_SENSOR message. + * + * @param hil_sensor Reference to the HIL_SENSOR message where the acceleration data will be set. + */ void MAVLinkManager::setAccelerationData(mavlink_hil_sensor_t& hil_sensor) { hil_sensor.xacc = DataRefManager::getFloat("sim/flightmodel/forces/g_axil") * DataRefManager::g_earth * -1; hil_sensor.yacc = DataRefManager::getFloat("sim/flightmodel/forces/g_side") * DataRefManager::g_earth * -1; hil_sensor.zacc = DataRefManager::getFloat("sim/flightmodel/forces/g_nrml") * DataRefManager::g_earth * -1; } +/** + * @brief Sets the gyroscope data in the HIL_SENSOR message. + * + * This function retrieves the aircraft's current rotational rates (in radians) from the simulation + * and sets them in the provided HIL_SENSOR message. + * + * @param hil_sensor Reference to the HIL_SENSOR message where the gyroscope data will be set. + */ void MAVLinkManager::setGyroData(mavlink_hil_sensor_t& hil_sensor) { hil_sensor.xgyro = DataRefManager::getFloat("sim/flightmodel/position/Prad"); hil_sensor.ygyro = DataRefManager::getFloat("sim/flightmodel/position/Qrad"); hil_sensor.zgyro = DataRefManager::getFloat("sim/flightmodel/position/Rrad"); } +/** + * @brief Sets the pressure data in the HIL_SENSOR message. + * + * This function retrieves the aircraft's current barometric pressure and pressure altitude from the simulation. + * It also adds noise to the barometric pressure reading to simulate real-world sensor inaccuracies. + * The resulting pressure data is then set in the provided HIL_SENSOR message. + * + * @param hil_sensor Reference to the HIL_SENSOR message where the pressure data will be set. + */ void MAVLinkManager::setPressureData(mavlink_hil_sensor_t& hil_sensor) { float basePressure = DataRefManager::getFloat("sim/weather/barometer_current_inhg") * 33.8639; float pressureNoise = noiseDistribution(gen); @@ -415,36 +439,48 @@ void MAVLinkManager::setPressureData(mavlink_hil_sensor_t& hil_sensor) { hil_sensor.diff_pressure = 0; } + +/** + * @brief Sets the magnetic field data in the HIL_SENSOR message. + * + * This function retrieves the aircraft's current orientation and position, then uses the precalculated + * Earth's magnetic field in NED coordinates. It rotates this magnetic field to the body frame of the aircraft + * and adds noise to simulate real-world magnetometer readings. The resulting magnetic field data is then set + * in the provided HIL_SENSOR message. + * + * @param hil_sensor Reference to the HIL_SENSOR message where the magnetic field data will be set. + */ void MAVLinkManager::setMagneticFieldData(mavlink_hil_sensor_t& hil_sensor) { - // Get the heading, roll, and pitch + // Retrieve the aircraft's current heading, roll, and pitch from the simulation float yaw_mag = DataRefManager::getFloat("sim/flightmodel/position/mag_psi"); float roll = DataRefManager::getFloat("sim/flightmodel/position/phi"); float pitch = DataRefManager::getFloat("sim/flightmodel/position/theta"); - // Convert to radians + // Convert the retrieved angles from degrees to radians for further calculations float yaw_rad = yaw_mag * M_PI / 180.0f; float roll_rad = roll * M_PI / 180.0f; float pitch_rad = pitch * M_PI / 180.0f; - // Get the Location + // Retrieve the aircraft's current geodetic position from the simulation float latitude = DataRefManager::getFloat("sim/flightmodel/position/latitude"); float longitude = DataRefManager::getFloat("sim/flightmodel/position/longitude"); float altitude = DataRefManager::getFloat("sim/flightmodel/position/elevation"); - // Call the calculateMagneticVector function - Eigen::Vector3f B_body = MAVLinkManager::calculateMagneticVector(latitude, longitude, altitude, roll_rad, pitch_rad, yaw_rad); + // Rotate the precalculated Earth's magnetic field from NED to the aircraft's body frame + Eigen::Vector3f bodyMagneticField = DataRefManager::convertNEDToBody(DataRefManager::earthMagneticFieldNED, roll_rad, pitch_rad, yaw_rad); - // Generate random noise values for the magnetometer data + // Generate random noise values to simulate real-world magnetometer inaccuracies float xmagNoise = noiseDistribution_mag(gen); float ymagNoise = noiseDistribution_mag(gen); float zmagNoise = noiseDistribution_mag(gen); - // Set the magnetic field in the HIL_SENSOR message with added noise - hil_sensor.xmag = B_body(0) + xmagNoise; - hil_sensor.ymag = B_body(1) + ymagNoise; - hil_sensor.zmag = B_body(2) + zmagNoise; + // Set the noisy magnetic field readings in the HIL_SENSOR message + hil_sensor.xmag = bodyMagneticField(0) + xmagNoise; + hil_sensor.ymag = bodyMagneticField(1) + ymagNoise; + hil_sensor.zmag = bodyMagneticField(2) + zmagNoise; } + /** * @brief Sets the time and fix type data for the HIL_GPS message. *