organizing

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.");
 

DataRefManager.cpp

@@ -69,6 +69,77 @@ std::vector<DataRefItem> DataRefManager::dataRefs = {
 	{"RC Information", "Rudder", "sim/joystick/yoke_heading_ratio", "", 1, DRT_FLOAT, std::function<float(const char*)>(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) {
 

DataRefManager.h

@@ -9,7 +9,9 @@
 #include <string>
 #include<cmath>
 #include<vector>
-
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <Eigen/Core>
 
 enum DataRefType {
     DRT_FLOAT,
@@ -35,6 +37,8 @@ class DataRefManager {
 public:
     static constexpr float g_earth = 9.81f;
     static std::vector<DataRefItem> dataRefs;
+    static Eigen::Vector3f earthMagneticFieldNED;
+
 
     static void drawDataRefs(XPLMWindowID in_window_id, int l, int t, float col_white[], int lineOffset);
     static std::vector<float> 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();
 
 
 };

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.
  *