diff --git a/include/TimeManager.h b/include/TimeManager.h new file mode 100644 index 0000000..fa5d36a --- /dev/null +++ b/include/TimeManager.h @@ -0,0 +1,23 @@ +#pragma once +#ifndef TIMEMANAGER_H +#define TIMEMANAGER_H + +#include + +class TimeManager { +public: + // Get the current time in microseconds since epoch + static uint64_t getCurrentTimeUsec(); + + // Get the current time in seconds since epoch (with fractional seconds) + static double getCurrentTimeSec(); // <-- Add this line + + // Calculate elapsed time in microseconds since the last call + static uint64_t calculateElapsedTimeUsec(uint64_t& lastTime); + +private: + // Use a high-resolution clock for time measurements + using HighResClock = std::chrono::high_resolution_clock; +}; + +#endif // TIMEMANAGER_H diff --git a/px4-xplane.vcxproj b/px4-xplane.vcxproj index 1d0f0e6..f123169 100644 --- a/px4-xplane.vcxproj +++ b/px4-xplane.vcxproj @@ -163,6 +163,7 @@ + @@ -170,6 +171,7 @@ + diff --git a/src/ConfigManager.cpp b/src/ConfigManager.cpp index 1defc64..d833ef7 100644 --- a/src/ConfigManager.cpp +++ b/src/ConfigManager.cpp @@ -28,7 +28,7 @@ std::string ConfigManager::configName; * to changes but may be less effective at eliminating noise. The optimal window * size should be chosen based on the specific application and expected noise characteristics. */ -int ConfigManager::MEDIAN_FILTER_WINDOW_SIZE = 3; +int ConfigManager::MEDIAN_FILTER_WINDOW_SIZE = 4; /** * @brief Flag for enabling/disabling velocity filtering. @@ -62,7 +62,7 @@ float ConfigManager::velocity_filter_alpha = 0.99f; * more reliable state estimation in the EKF. When disabled, the raw accelerometer data * is used without filtering. */ -bool ConfigManager::filter_accel_enabled = true; +bool ConfigManager::filter_accel_enabled = false; /** * @brief Alpha value for accelerometer filtering. @@ -84,7 +84,7 @@ float ConfigManager::accel_filter_alpha = 0.99f; * are applied to the barometric pressure data to smooth the input and reduce noise. This ensures * more stable pressure readings and reduces the impact of noisy sensor data on state estimation. */ -bool ConfigManager::filter_barometer_enabled = false; +bool ConfigManager::filter_barometer_enabled = true; /** * @brief Alpha value for barometer filtering. diff --git a/src/MAVLinkManager.cpp b/src/MAVLinkManager.cpp index 03eff97..b87d43f 100644 --- a/src/MAVLinkManager.cpp +++ b/src/MAVLinkManager.cpp @@ -6,6 +6,7 @@ #include // for std::tuple #include #include "FilterUtils.h" +#include "TimeManager.h" // Include the TimeManager class // Define and initialize the static member @@ -20,8 +21,9 @@ constexpr float GRAVITY = 9.81; // Define and initialize the random number generators and distributions std::random_device MAVLinkManager::rd; std::mt19937 MAVLinkManager::gen(MAVLinkManager::rd()); -std::normal_distribution MAVLinkManager::highFreqNoise(0.0f, 0.0001f); -std::normal_distribution MAVLinkManager::lowFreqNoise(0.0f, 0.0005f); // Larger, slower noise +std::normal_distribution MAVLinkManager::highFreqNoise(0.0f, 0.00003f); +std::normal_distribution MAVLinkManager::lowFreqNoise(0.0f, 0.000003f); // Larger, slower noise +std::normal_distribution vibrationNoise(0.0f, 0.2f); // Adjust the standard deviation as needed std::normal_distribution MAVLinkManager::noiseDistribution_mag(0.0f, 0.000001f); @@ -61,14 +63,15 @@ std::normal_distribution MAVLinkManager::noiseDistribution_mag(0.0f, 0.00 * @see setPressureData * @see setMagneticFieldData */ -void MAVLinkManager::sendHILSensor(uint8_t sensor_id=0) { +void MAVLinkManager::sendHILSensor(uint8_t sensor_id = 0) { if (!ConnectionManager::isConnected()) return; mavlink_message_t msg; mavlink_hil_sensor_t hil_sensor; - hil_sensor.time_usec = static_cast(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6); + // Use the high-resolution time from the TimeManager utility + hil_sensor.time_usec = TimeManager::getCurrentTimeUsec(); hil_sensor.id = uint8_t(sensor_id); setAccelerationData(hil_sensor); @@ -78,20 +81,17 @@ void MAVLinkManager::sendHILSensor(uint8_t sensor_id=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 - fields_updated |= (1 << 1); // HIL_SENSOR_UPDATED_YACC - fields_updated |= (1 << 2); // HIL_SENSOR_UPDATED_ZACC - fields_updated |= (1 << 3); // HIL_SENSOR_UPDATED_XGYRO - fields_updated |= (1 << 4); // HIL_SENSOR_UPDATED_YGYRO - fields_updated |= (1 << 5); // HIL_SENSOR_UPDATED_ZGYRO - fields_updated |= (1 << 6); // HIL_SENSOR_UPDATED_XMAG - fields_updated |= (1 << 7); // HIL_SENSOR_UPDATED_YMAG - fields_updated |= (1 << 8); // HIL_SENSOR_UPDATED_ZMAG - fields_updated |= (1 << 9); // HIL_SENSOR_UPDATED_ABS_PRESSURE + uint32_t fields_updated = 0; + fields_updated |= (1 << 0); // HIL_SENSOR_UPDATED_XACC + fields_updated |= (1 << 1); // HIL_SENSOR_UPDATED_YACC + fields_updated |= (1 << 2); // HIL_SENSOR_UPDATED_ZACC + fields_updated |= (1 << 3); // HIL_SENSOR_UPDATED_XGYRO + fields_updated |= (1 << 4); // HIL_SENSOR_UPDATED_YGYRO + fields_updated |= (1 << 5); // HIL_SENSOR_UPDATED_ZGYRO + fields_updated |= (1 << 6); // HIL_SENSOR_UPDATED_XMAG + fields_updated |= (1 << 7); // HIL_SENSOR_UPDATED_YMAG + fields_updated |= (1 << 8); // HIL_SENSOR_UPDATED_ZMAG + fields_updated |= (1 << 9); // HIL_SENSOR_UPDATED_ABS_PRESSURE fields_updated |= (1 << 10); // HIL_SENSOR_UPDATED_DIF_PRESSURE fields_updated |= (1 << 11); // HIL_SENSOR_UPDATED_PRESSURE_ALT fields_updated |= (1 << 12); // HIL_SENSOR_UPDATED_TEMPERATURE @@ -105,6 +105,7 @@ void MAVLinkManager::sendHILSensor(uint8_t sensor_id=0) { } + /** * @brief Sends the HIL_GPS MAVLink message. * @@ -145,6 +146,9 @@ void MAVLinkManager::sendHILGPS() { mavlink_message_t msg; mavlink_hil_gps_t hil_gps; + // Use the high-resolution time from the TimeManager utility + hil_gps.time_usec = TimeManager::getCurrentTimeUsec(); + // Set various GPS data components setGPSTimeAndFix(hil_gps); setGPSPositionData(hil_gps); @@ -164,6 +168,7 @@ void MAVLinkManager::sendHILGPS() { + /** * @brief Checks if the Earth's magnetic field needs to be updated and updates it if necessary. * @@ -233,42 +238,47 @@ void MAVLinkManager::sendHILStateQuaternion() { * @param hil_state Reference to the MAVLink HIL state quaternion structure to be populated. */ void MAVLinkManager::populateHILStateQuaternion(mavlink_hil_state_quaternion_t& hil_state) { - hil_state.time_usec = static_cast(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6); + // Use the high-resolution time from the TimeManager utility + hil_state.time_usec = TimeManager::getCurrentTimeUsec(); + // Retrieve quaternion data from X-Plane and populate the MAVLink message std::vector q = DataRefManager::getFloatArray("sim/flightmodel/position/q"); for (int i = 0; i < 4; ++i) { hil_state.attitude_quaternion[i] = q[i]; } + // Retrieve angular velocity data (roll, pitch, yaw rates) 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"); + // Retrieve position data (latitude, longitude, altitude) hil_state.lat = static_cast(DataRefManager::getFloat("sim/flightmodel/position/latitude") * 1e7); hil_state.lon = static_cast(DataRefManager::getFloat("sim/flightmodel/position/longitude") * 1e7); hil_state.alt = static_cast(DataRefManager::getFloat("sim/flightmodel/position/elevation") * 1e3); - // Convert OGL local velocities to NED frame + // Convert local OGL velocities to NED frame float ogl_vx = DataRefManager::getFloat("sim/flightmodel/position/local_vx") * 100; float ogl_vy = DataRefManager::getFloat("sim/flightmodel/position/local_vy") * 100; float ogl_vz = DataRefManager::getFloat("sim/flightmodel/position/local_vz") * 100; - // Local OGL to NED transformation: - // OGL: X: East, Y: Up, Z: South - // NED: North, East, Down - hil_state.vx = -1 * ogl_vz; // North (NED) from South (OGL) - hil_state.vy = ogl_vx; // East (NED) from East (OGL) - hil_state.vz = -1 * ogl_vy; // Down (NED) from Up (OGL) + // OGL to NED transformation: + hil_state.vx = -ogl_vz; // North (NED) from South (OGL) + hil_state.vy = ogl_vx; // East (NED) from East (OGL) + hil_state.vz = -ogl_vy; // Down (NED) from Up (OGL) + // Retrieve airspeed data (indicated and true airspeed) hil_state.ind_airspeed = static_cast(DataRefManager::getFloat("sim/flightmodel/position/indicated_airspeed") * 100); hil_state.true_airspeed = static_cast(DataRefManager::getFloat("sim/flightmodel/position/true_airspeed") * 100); + // Retrieve acceleration data and convert to m/s^2 hil_state.xacc = DataRefManager::getFloat("sim/flightmodel/forces/g_axil") * DataRefManager::g_earth * -1; hil_state.yacc = DataRefManager::getFloat("sim/flightmodel/forces/g_side") * DataRefManager::g_earth * -1; hil_state.zacc = DataRefManager::getFloat("sim/flightmodel/forces/g_nrml") * DataRefManager::g_earth * -1; } + /** * @brief Sends the provided MAVLink message. * @@ -292,30 +302,43 @@ void MAVLinkManager::sendData(const mavlink_message_t& msg) { * Inputs message over the MAVLink connection. */ void MAVLinkManager::sendHILRCInputs() { + // Check if we're connected before proceeding if (!ConnectionManager::isConnected()) return; + // Create a MAVLink message and the corresponding HIL RC inputs structure mavlink_message_t msg; mavlink_hil_rc_inputs_raw_t hil_rc_inputs; - hil_rc_inputs.time_usec = static_cast(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6); + // Use the high-resolution time from the TimeManager utility + hil_rc_inputs.time_usec = TimeManager::getCurrentTimeUsec(); + + // Map RC channels using joystick data from X-Plane hil_rc_inputs.chan1_raw = mapRCChannel(DataRefManager::getFloat("sim/joystick/yoke_roll_ratio"), -1, +1); hil_rc_inputs.chan2_raw = mapRCChannel(DataRefManager::getFloat("sim/joystick/yoke_pitch_ratio"), -1, +1); hil_rc_inputs.chan3_raw = mapRCChannel(DataRefManager::getFloat("sim/cockpit2/engine/actuators/throttle_ratio_all"), 0, +1); hil_rc_inputs.chan4_raw = mapRCChannel(DataRefManager::getFloat("sim/joystick/yoke_heading_ratio"), -1, +1); - hil_rc_inputs.chan5_raw = mapRCChannel(0, -1, +1); // Example for a channel with a constant value - // Map the remaining channels as needed + // Additional channels, as needed (e.g., auxiliary channels, mode switch) + hil_rc_inputs.chan5_raw = mapRCChannel(0, -1, +1); // Placeholder for an unused or fixed-value channel + // Add mappings for other channels as needed + // hil_rc_inputs.chan6_raw = mapRCChannel(...); + // hil_rc_inputs.chan7_raw = mapRCChannel(...); + // hil_rc_inputs.chan8_raw = mapRCChannel(...); + + // Set RSSI (Received Signal Strength Indicator) to maximum value constexpr uint8_t RSSI_MAX_VALUE = 255; - hil_rc_inputs.rssi = RSSI_MAX_VALUE; // Set the RSSI field to its maximum value + hil_rc_inputs.rssi = RSSI_MAX_VALUE; // Full signal strength + // Encode the MAVLink message with the populated HIL RC inputs structure mavlink_msg_hil_rc_inputs_raw_encode(1, 1, &msg, &hil_rc_inputs); + // Send the MAVLink message buffer uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; int len = mavlink_msg_to_send_buffer(buffer, &msg); - ConnectionManager::sendData(buffer, len); } + /** * @brief Maps the RC channel value to the MAVLink format. * @@ -435,8 +458,32 @@ void MAVLinkManager::processHILActuatorControlsMessage(const mavlink_message_t& void MAVLinkManager::setAccelerationData(mavlink_hil_sensor_t& hil_sensor) { // Retrieve raw accelerometer data from X-Plane float raw_xacc = DataRefManager::getFloat("sim/flightmodel/forces/g_axil") * DataRefManager::g_earth * -1; - float raw_yacc = DataRefManager::getFloat("sim/flightmodel/forces/g_side") * DataRefManager::g_earth * -1; + float raw_yacc = DataRefManager::getFloat("sim/flightmodel/forces/g_side") * DataRefManager::g_earth * -11; float raw_zacc = DataRefManager::getFloat("sim/flightmodel/forces/g_nrml") * DataRefManager::g_earth * -1; + /* + // Add realistic ground vibration + float vibration_frequency = 50.0f; // Frequency of vibration (Hz) + float vibration_amplitude = 0.05f; // Amplitude of vibration (m/s^2) + float noise_amplitude = 0.01f; // Amplitude of noise (m/s^2) + + // Get the current time in seconds + double currentTimeSec = TimeManager::getCurrentTimeSec(); + + // Simulate vibration using a simple sinusoidal function plus noise + float vibration_x = vibration_amplitude * sin(2 * M_PI * vibration_frequency * currentTimeSec) + MAVLinkManager::highFreqNoise(gen) * noise_amplitude; + float vibration_y = vibration_amplitude * sin(2 * M_PI * vibration_frequency * currentTimeSec + M_PI / 2) + MAVLinkManager::highFreqNoise(gen) * noise_amplitude; + float vibration_z = vibration_amplitude * sin(2 * M_PI * vibration_frequency * currentTimeSec + M_PI) + MAVLinkManager::highFreqNoise(gen) * noise_amplitude; + + // Add the vibration to the raw accelerometer data + raw_xacc += vibration_x; + raw_yacc += vibration_y; + raw_zacc += vibration_z; + */ + + raw_xacc += vibrationNoise(gen); + raw_yacc += vibrationNoise(gen); + raw_zacc += vibrationNoise(gen); + // Apply filtering if enabled, otherwise use raw data hil_sensor.xacc = DataRefManager::applyFilteringIfNeeded(raw_xacc, ConfigManager::filter_accel_enabled, ConfigManager::accel_filter_alpha, DataRefManager::median_filter_window_xacc); @@ -448,6 +495,7 @@ void MAVLinkManager::setAccelerationData(mavlink_hil_sensor_t& hil_sensor) { + /** * @brief Sets the gyroscope data in the HIL_SENSOR message. * @@ -477,31 +525,42 @@ void MAVLinkManager::setGyroData(mavlink_hil_sensor_t& hil_sensor) { * @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) { + // Convert the barometric pressure from inHg to hPa float basePressure = DataRefManager::getFloat("sim/weather/barometer_current_inhg") * 33.8639; - // Apply a combination of high-frequency and low-frequency noise - float highFreqNoise_term = MAVLinkManager::highFreqNoise(gen); - float lowFreqNoise_term = MAVLinkManager::lowFreqNoise(gen) * sin(XPLMGetDataf(XPLMFindDataRef("sim/time/total_flight_time_sec"))); - float pressureNoise = highFreqNoise_term + lowFreqNoise_term; + // Retrieve the precise current time using the TimeManager + float currentTimeSec = static_cast(TimeManager::getCurrentTimeSec()); + + // Generate high-frequency noise + float highFreqNoiseTerm = MAVLinkManager::highFreqNoise(gen); + + // Generate low-frequency noise with a sine wave pattern for realism + float lowFreqNoiseTerm = MAVLinkManager::lowFreqNoise(gen) * sin(currentTimeSec); + + // Combine both noise components + float pressureNoise = highFreqNoiseTerm + lowFreqNoiseTerm; + + // Add the noise to the base pressure reading float noisyPressure = basePressure + pressureNoise; - static float previousPressure = 0; - + + // Calculate the pressure altitude based on the noisy pressure float pressureAltitude = DataRefManager::calculatePressureAltitude(noisyPressure); + // Set the pressure and altitude values in the HIL_SENSOR message hil_sensor.abs_pressure = noisyPressure; hil_sensor.pressure_alt = pressureAltitude; - float ias = DataRefManager::getFloat("sim/flightmodel/position/indicated_airspeed") * 0.514444; - float dynamicPressure = 0.01 * (0.5 * DataRefManager::AirDensitySeaLevel * ias * ias); + // Calculate differential pressure using the indicated airspeed (IAS) + float ias = DataRefManager::getFloat("sim/flightmodel/position/indicated_airspeed") * 0.514444; // Convert IAS to m/s + float dynamicPressure = 0.01f * (0.5f * DataRefManager::AirDensitySeaLevel * ias * ias); // Dynamic pressure in hPa hil_sensor.diff_pressure = dynamicPressure; - - previousPressure = noisyPressure; } + /** * @brief Sets the magnetic field data in the HIL_SENSOR message. * @@ -543,13 +602,21 @@ void MAVLinkManager::setMagneticFieldData(mavlink_hil_sensor_t& hil_sensor) { /** * @brief Sets the time and fix type data for the HIL_GPS message. * + * This function uses the TimeManager utility to set a high-resolution time stamp + * and assumes a 3D fix for the GPS data. The fix type can be modified in the future + * to be more dynamic if needed. + * * @param hil_gps Reference to the mavlink_hil_gps_t structure to populate. */ void MAVLinkManager::setGPSTimeAndFix(mavlink_hil_gps_t& hil_gps) { - hil_gps.time_usec = static_cast(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6); - hil_gps.fix_type = static_cast(3); // Assuming always a 3D fix in the simulation environment + // Set the time using the high-resolution time from TimeManager + hil_gps.time_usec = TimeManager::getCurrentTimeUsec(); + + // Set the GPS fix type to 3D fix (value 3) + hil_gps.fix_type = static_cast(3); // Assuming a 3D fix in the simulation environment } + /** * @brief Sets the position data for the HIL_GPS message. * @@ -710,6 +777,8 @@ void MAVLinkManager::setGPSHeadingData(mavlink_hil_gps_t& hil_gps) { uint16_t cog = static_cast(DataRefManager::getFloat("sim/cockpit2/gauges/indicators/ground_track_mag_copilot") * 100); //uint16_t cog = static_cast(DataRefManager::getFloat("sim/flightmodel/position/hpath") * 100.0f); hil_gps.cog = (cog == 36000) ? static_cast < uint16_t>(0001) : cog; - uint16_t yaw = static_cast(DataRefManager::getFloat("sim/flightmodel/position/mag_psi") * 100.0f); - hil_gps.yaw = (yaw == 0) ? static_cast < uint16_t>(35999) : yaw; + //uint16_t yaw = static_cast(DataRefManager::getFloat("sim/flightmodel/position/mag_psi") * 100.0f); + //hil_gps.yaw = (yaw == 0) ? static_cast < uint16_t>(35999) : yaw; + uint16_t yaw = static_cast(0); + } \ No newline at end of file diff --git a/src/TimeManager.cpp b/src/TimeManager.cpp new file mode 100644 index 0000000..8fddeca --- /dev/null +++ b/src/TimeManager.cpp @@ -0,0 +1,20 @@ +#include "TimeManager.h" + +uint64_t TimeManager::getCurrentTimeUsec() { + auto now = HighResClock::now(); + auto duration = now.time_since_epoch(); + return std::chrono::duration_cast(duration).count(); +} + +double TimeManager::getCurrentTimeSec() { // <-- Implement this function + auto now = HighResClock::now(); + auto duration = now.time_since_epoch(); + return std::chrono::duration_cast>(duration).count(); +} + +uint64_t TimeManager::calculateElapsedTimeUsec(uint64_t& lastTime) { + uint64_t currentTime = getCurrentTimeUsec(); + uint64_t elapsedTime = currentTime - lastTime; + lastTime = currentTime; + return elapsedTime; +} diff --git a/src/px4xplane.cpp b/src/px4xplane.cpp index ce2d7b5..58fb9e3 100644 --- a/src/px4xplane.cpp +++ b/src/px4xplane.cpp @@ -347,7 +347,7 @@ void create_menu() { XPLMAppendMenuItem(airframesMenu, menuItemName.c_str(), (void*)(intptr_t)i, 1); } - //XPLMAppendMenuItem(g_menu_id, "Show Data", (void*)0, 1); + XPLMAppendMenuItem(g_menu_id, "Show Data", (void*)0, 1); toggleEnableCmd = XPLMCreateCommand("px4xplane/toggleEnable", "Toggle enable/disable state"); XPLMRegisterCommandHandler(toggleEnableCmd, toggleEnableHandler, 1, (void*)0); XPLMAppendMenuSeparator(g_menu_id); @@ -458,6 +458,7 @@ float MyFlightLoopCallback(float inElapsedSinceLastCall, float inElapsedTimeSinc timeSinceLastRcUpdate = 0.0f; } + // Continuously receive and process actuator data ConnectionManager::receiveData();