fixed many prolems. now atitude failure exists. maybe accel/gyro/quat…

…ernion problem + no compass and no RC Data yet

ConnectionManager.cpp

@@ -127,6 +127,14 @@ void ConnectionManager::closeSocket(int& sockfd) {
 }
 void ConnectionManager::sendData(const uint8_t* buffer, int len) {
     if (!connected) return;
+
+    // Log the MAVLink packet in an interpretable format
+    std::string logMessage = "Sending MAVLink packet: ";
+    /*for (int i = 0; i < len; i++) {
+        logMessage += std::to_string(buffer[i]) + " ";
+    }*/
+    XPLMDebugString(logMessage.c_str());
+
     int totalBytesSent = 0;
     while (totalBytesSent < len) {
         int bytesSent = send(newsockfd, reinterpret_cast<const char*>(buffer) + totalBytesSent, len - totalBytesSent, 0);

MAVLinkManager.cpp

@@ -2,6 +2,8 @@
 #include "mavlink/c_library_v2/common/mavlink.h"
 #include "ConnectionManager.h"
 #include "DataRefManager.h"
+#include <random>
+#include <vector>
 
 void MAVLinkManager::sendHILSensor() {
     if (!ConnectionManager::isConnected()) return;
@@ -10,7 +12,7 @@ void MAVLinkManager::sendHILSensor() {
     mavlink_hil_sensor_t hil_sensor;
 
     hil_sensor.time_usec = static_cast<uint64_t>(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6);
-
+    hil_sensor.id = uint8_t(0);
     hil_sensor.xacc = DataRefManager::getFloat("sim/flightmodel/forces/g_axil") * DataRefManager::g_earth;
     hil_sensor.yacc = DataRefManager::getFloat("sim/flightmodel/forces/g_side") * DataRefManager::g_earth;
     hil_sensor.zacc = DataRefManager::getFloat("sim/flightmodel/forces/g_nrml") * DataRefManager::g_earth;
@@ -19,7 +21,18 @@ void MAVLinkManager::sendHILSensor() {
     hil_sensor.ygyro = DataRefManager::getFloat("sim/flightmodel/position/Qrad");
     hil_sensor.zgyro = DataRefManager::getFloat("sim/flightmodel/position/Rrad");
 
-    hil_sensor.abs_pressure = DataRefManager::getFloat("sim/weather/barometer_current_inhg") * 33.8639;
+    // Get the base pressure value
+    float basePressure = DataRefManager::getFloat("sim/weather/barometer_current_inhg") * 33.8639;
+
+    // Generate a random noise value
+    std::random_device rd;
+    std::mt19937 gen(rd());
+    std::normal_distribution<float> noiseDistribution(0.0f, 0.01f); // Adjust the standard deviation as needed
+
+    // Add noise to the pressure value
+    float pressureNoise = noiseDistribution(gen);
+    hil_sensor.abs_pressure = basePressure + pressureNoise;
+
     hil_sensor.pressure_alt = DataRefManager::getDouble("sim/flightmodel2/position/pressure_altitude") * 0.3048;
     // Find suitable datarefs or provide default values
     hil_sensor.diff_pressure = 0;
@@ -60,16 +73,16 @@ void MAVLinkManager::sendHILGPS() {
     mavlink_hil_gps_t hil_gps;
 
     hil_gps.time_usec = static_cast<uint64_t>(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6); // converted to us
-    hil_gps.fix_type = 3; // 3: 3D fix, since we are in a simulation environment, we can assume we always have a 3D fix
+    hil_gps.fix_type = 3; // 3: 3D fix, assuming we always have a 3D fix in the simulation environment
 
     hil_gps.lat = static_cast<int32_t>(DataRefManager::getFloat("sim/flightmodel/position/latitude") * 1e7); // converted to degE7
     hil_gps.lon = static_cast<int32_t>(DataRefManager::getFloat("sim/flightmodel/position/longitude") * 1e7); // converted to degE7
 
     hil_gps.alt = static_cast<int32_t>(DataRefManager::getFloat("sim/flightmodel/position/elevation") * 1e3); // converted to mm
 
-    hil_gps.eph = 20; // Example, 20: equivalent to 0.2 in HDOP, assuming very high accuracy due to simulation environment
-    hil_gps.epv = 20; // Example, 20: equivalent to 0.2 in VDOP, assuming very high accuracy due to simulation environment
-    hil_gps.satellites_visible = 12; // Example, assuming we have 12 satellites visible as it’s common in good conditions.
+    hil_gps.eph = 20; // Example: equivalent to 0.2 in HDOP, assuming very high accuracy due to simulation environment
+    hil_gps.epv = 20; // Example: equivalent to 0.2 in VDOP, assuming very high accuracy due to simulation environment
+    hil_gps.satellites_visible = 12; // Example: assuming we have 12 satellites visible as it's common in good conditions.
 
     hil_gps.vn = static_cast<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_vx") * 100); // converted to cm/s
     hil_gps.ve = static_cast<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_vy") * 100); // converted to cm/s
@@ -86,3 +99,46 @@ void MAVLinkManager::sendHILGPS() {
     int len = mavlink_msg_to_send_buffer(buffer, &msg);
     ConnectionManager::sendData(buffer, len);
 }
+
+
+void MAVLinkManager::sendHILStateQuaternion() {
+
+    if (!ConnectionManager::isConnected()) return;
+
+    mavlink_message_t msg;
+    mavlink_hil_state_quaternion_t hil_state;
+
+    hil_state.time_usec = static_cast<uint64_t>(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6);
+    std::vector<float> q = DataRefManager::getFloatArray("sim/flightmodel/position/q");
+    hil_state.attitude_quaternion[0] = q[0];
+    hil_state.attitude_quaternion[1] = q[1];
+    hil_state.attitude_quaternion[2] = q[2];
+    hil_state.attitude_quaternion[3] = q[3];
+
+    hil_state.rollspeed = DataRefManager::getFloat("sim/flightmodel/position/Prad");
+    hil_state.pitchspeed = DataRefManager::getFloat("sim/flightmodel/position/Qrad");
+    hil_state.yawspeed = DataRefManager::getFloat("sim/flightmodel/position/Rrad");
+
+    hil_state.lat = static_cast<int32_t>(DataRefManager::getFloat("sim/flightmodel/position/latitude") * 1e7);
+    hil_state.lon = static_cast<int32_t>(DataRefManager::getFloat("sim/flightmodel/position/longitude") * 1e7);
+
+    hil_state.alt = static_cast<int32_t>(DataRefManager::getFloat("sim/flightmodel/position/elevation") * 1e3);
+
+    hil_state.vx = static_cast<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_vx") * 100);
+    hil_state.vy = static_cast<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_vy") * 100);
+    hil_state.vz = static_cast<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_vz") * 100);
+
+    hil_state.ind_airspeed = static_cast<uint16_t>(DataRefManager::getFloat("sim/flightmodel/position/indicated_airspeed") * 100);
+    hil_state.true_airspeed = static_cast<uint16_t>(DataRefManager::getFloat("sim/flightmodel/position/true_airspeed") * 100);
+
+    hil_state.xacc = static_cast<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_ax") * 1000 / 9.81);
+    hil_state.yacc = static_cast<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_ay") * 1000 / 9.81);
+    hil_state.zacc = static_cast<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_az") * 1000 / 9.81);
+
+    mavlink_msg_hil_state_quaternion_encode(1, 1, &msg, &hil_state);
+
+    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+    int len = mavlink_msg_to_send_buffer(buffer, &msg);
+
+    ConnectionManager::sendData(buffer, len);
+}

MAVLinkManager.h

@@ -5,5 +5,6 @@ class MAVLinkManager {
     public:
     static void sendHILSensor();
     static void sendHILGPS();
+    static void sendHILStateQuaternion();
 
 };

px4xplane.cpp

@@ -244,7 +244,8 @@ float MyFlightLoopCallback(float inElapsedSinceLastCall, float inElapsedTimeSinc
 
 	// Call MAVLinkManager::sendHILSensor() to send HIL_SENSOR data
 	MAVLinkManager::sendHILSensor();
-	//MAVLinkManager::sendHILGPS();
+	MAVLinkManager::sendHILGPS();
+	MAVLinkManager::sendHILStateQuaternion();
 
 
 	//ConnectionManager::receiveData();