diff --git a/DataRefManager.cpp b/DataRefManager.cpp index b67a785..a72f05d 100644 --- a/DataRefManager.cpp +++ b/DataRefManager.cpp @@ -14,6 +14,7 @@ std::vector DataRefManager::dataRefs = { {"Position", "Longitude", "sim/flightmodel/position/longitude", "deg", 1, DRT_FLOAT, std::function(getFloat)}, {"Position", "Longitude", "sim/flightmodel/position/longitude", "deg", 1, DRT_FLOAT, std::function(getFloat)}, {"Position", "Altitude", "sim/flightmodel/position/elevation", "m", 1, DRT_FLOAT, std::function(getFloat)}, + {"Position", "Course (Track)", "sim/cockpit2/gauges/indicators/ground_track_mag_copilot", "deg", 1, DRT_FLOAT, std::function(getFloat)}, // Attitude Information {"Attitude", "Pitch", "sim/flightmodel/position/theta", "deg", 1, DRT_FLOAT, std::function(getFloat)}, diff --git a/MAVLinkManager.cpp b/MAVLinkManager.cpp index a394724..b205861 100644 --- a/MAVLinkManager.cpp +++ b/MAVLinkManager.cpp @@ -1,6 +1,7 @@ #include "MAVLinkManager.h" -#include "mavlink/c_library_v2/all/mavlink.h" +#include "mavlink/c_library_v2/common/mavlink.h" #include "ConnectionManager.h" +#include "DataRefManager.h" void MAVLinkManager::sendHILSensor() { if (!ConnectionManager::isConnected()) return; @@ -8,25 +9,80 @@ void MAVLinkManager::sendHILSensor() { mavlink_message_t msg; mavlink_hil_sensor_t hil_sensor; - // Fill in the hil_sensor data with dummy values - hil_sensor.time_usec = 0; - hil_sensor.xacc = 1.0f; - hil_sensor.yacc = 1.0f; - hil_sensor.zacc = 1.0f; - hil_sensor.xgyro = 1.0f; - hil_sensor.ygyro = 1.0f; - hil_sensor.zgyro = 1.0f; - hil_sensor.xmag = 1.0f; - hil_sensor.ymag = 1.0f; - hil_sensor.zmag = 1.0f; - hil_sensor.abs_pressure = 1.0f; - hil_sensor.diff_pressure = 1.0f; - hil_sensor.pressure_alt = 1.0f; - hil_sensor.temperature = 20.0f; - hil_sensor.fields_updated = 0; + hil_sensor.time_usec = static_cast(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6); + + 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; + + 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"); + + hil_sensor.abs_pressure = DataRefManager::getFloat("sim/weather/barometer_current_inhg") * 33.8639; + 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; + hil_sensor.xmag = 0; + hil_sensor.ymag = 0; + hil_sensor.zmag = 0; + + hil_sensor.temperature = DataRefManager::getFloat("sim/cockpit2/temperature/outside_air_temp_degc"); + + // Now set the bits for the fields you are updating. + // Refer to the provided enum values for the fields_updated flags. + //https://mavlink.io/en/messages/common.html#HIL_SENSOR_UPDATED_FLAGS + uint32_t fields_updated = 0; // Start with a bitmask of all zeros + fields_updated |= (1 << 0); // HIL_SENSOR_UPDATED_XACC, the bit shift corresponds to setting the 0th bit to 1 + fields_updated |= (1 << 1); // HIL_SENSOR_UPDATED_YACC, the bit shift corresponds to setting the 1st bit to 1 + fields_updated |= (1 << 2); // HIL_SENSOR_UPDATED_ZACC, the bit shift corresponds to setting the 2nd bit to 1 + fields_updated |= (1 << 3); // HIL_SENSOR_UPDATED_XGYRO, the bit shift corresponds to setting the 3rd bit to 1 + fields_updated |= (1 << 4); // HIL_SENSOR_UPDATED_YGYRO, the bit shift corresponds to setting the 4th bit to 1 + fields_updated |= (1 << 5); // HIL_SENSOR_UPDATED_ZGYRO, the bit shift corresponds to setting the 5th bit to 1 + fields_updated |= (1 << 9); // HIL_SENSOR_UPDATED_ABS_PRESSURE, the bit shift corresponds to setting the 9th bit to 1 + fields_updated |= (1 << 11); // HIL_SENSOR_UPDATED_PRESSURE_ALT, the bit shift corresponds to setting the 11th bit to 1 + fields_updated |= (1 << 12); // HIL_SENSOR_UPDATED_TEMPERATURE, the bit shift corresponds to setting the 12th bit to 1 + + hil_sensor.fields_updated = fields_updated; + + // Finally, assign the bitmask to the hil_sensor message object + hil_sensor.fields_updated = fields_updated; mavlink_msg_hil_sensor_encode(1, 1, &msg, &hil_sensor); uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; int len = mavlink_msg_to_send_buffer(buffer, &msg); ConnectionManager::sendData(buffer, len); -} \ No newline at end of file +} +void MAVLinkManager::sendHILGPS() { + if (!ConnectionManager::isConnected()) return; + + mavlink_message_t msg; + mavlink_hil_gps_t hil_gps; + + hil_gps.time_usec = static_cast(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.lat = static_cast(DataRefManager::getFloat("sim/flightmodel/position/latitude") * 1e7); // converted to degE7 + hil_gps.lon = static_cast(DataRefManager::getFloat("sim/flightmodel/position/longitude") * 1e7); // converted to degE7 + + hil_gps.alt = static_cast(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.vn = static_cast(DataRefManager::getFloat("sim/flightmodel/position/local_vx") * 100); // converted to cm/s + hil_gps.ve = static_cast(DataRefManager::getFloat("sim/flightmodel/position/local_vy") * 100); // converted to cm/s + hil_gps.vd = static_cast(DataRefManager::getFloat("sim/flightmodel/position/local_vz") * 100); // converted to cm/s + + hil_gps.cog = static_cast(DataRefManager::getFloat("sim/cockpit2/gauges/indicators/ground_track_mag_copilot") * 100); // converted to cdeg + + hil_gps.vel = UINT16_MAX; // Assuming velocity is unknown as per MAVLink definition + hil_gps.id = 0; // 0 indexed GPS ID for single GPS + hil_gps.yaw = UINT16_MAX; // Yaw not available + + mavlink_msg_hil_gps_encode(1, 1, &msg, &hil_gps); + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + int len = mavlink_msg_to_send_buffer(buffer, &msg); + ConnectionManager::sendData(buffer, len); +} diff --git a/MAVLinkManager.h b/MAVLinkManager.h index 4dbef7b..618d3d2 100644 --- a/MAVLinkManager.h +++ b/MAVLinkManager.h @@ -4,4 +4,6 @@ class MAVLinkManager { public: static void sendHILSensor(); + static void sendHILGPS(); + }; \ No newline at end of file diff --git a/px4xplane.cpp b/px4xplane.cpp index f5d7bad..3b7a6c2 100644 --- a/px4xplane.cpp +++ b/px4xplane.cpp @@ -244,6 +244,7 @@ float MyFlightLoopCallback(float inElapsedSinceLastCall, float inElapsedTimeSinc // Call MAVLinkManager::sendHILSensor() to send HIL_SENSOR data MAVLinkManager::sendHILSensor(); + //MAVLinkManager::sendHILGPS(); //ConnectionManager::receiveData(); @@ -252,6 +253,9 @@ float MyFlightLoopCallback(float inElapsedSinceLastCall, float inElapsedTimeSinc PLUGIN_API void XPluginStop(void) { // Unregister the flight loop callback + if (ConnectionManager::isConnected) { + toggleEnable(); + } XPLMUnregisterFlightLoopCallback(MyFlightLoopCallback, NULL); #if IBM ConnectionManager::cleanupWinSock();