diff --git a/ConnectionManager.h b/ConnectionManager.h index e9641fe..68b93ed 100644 --- a/ConnectionManager.h +++ b/ConnectionManager.h @@ -25,4 +25,5 @@ class ConnectionManager { static std::string status; static int sitlPort; static std::string lastMessage; // Variable to keep the last message + }; \ No newline at end of file diff --git a/MAVLinkManager.cpp b/MAVLinkManager.cpp index e4c0268..930a8b7 100644 --- a/MAVLinkManager.cpp +++ b/MAVLinkManager.cpp @@ -1,35 +1,26 @@ #include "MAVLinkManager.h" -#include "mavlink/c_library_v2/common/mavlink.h" #include "ConnectionManager.h" #include "DataRefManager.h" -#include #include #include "XPLMUtilities.h" -#include -#include "XYZgeomag/src/XYZgeomag.hpp" -#include // for trigonometric functions and M_PI #include // for std::tuple -#include -#include // Define and initialize the static member MAVLinkManager::HILActuatorControlsData MAVLinkManager::hilActuatorControlsData = {}; -// Function to convert Euler angles (roll, pitch, yaw) to a rotation matrix -Eigen::Matrix3f eulerToRotationMatrix(float roll, float pitch, float yaw) { - Eigen::Matrix3f R; - float sr = sin(roll), cr = cos(roll); - float sp = sin(pitch), cp = cos(pitch); - float sy = sin(yaw), cy = cos(yaw); - R << cp * cy, (sr * sp * cy) - (cr * sy), (cr * sp * cy) + (sr * sy), - cp* sy, (sr * sp * sy) + (cr * cy), (cr * sp * sy) - (sr * cy), - -sp, sr* cp, cr* cp; - return R; -} +// Declare constants at the class or namespace level +constexpr float DEG_TO_RAD = 3.14159265358979323846 / 180.0; +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::noiseDistribution(0.0f, 0.01f); +std::normal_distribution MAVLinkManager::noiseDistribution_mag(0.0f, 0.0001f); @@ -50,7 +41,7 @@ Eigen::Matrix3f eulerToRotationMatrix(float roll, float pitch, float yaw) { * Positive yaw means turning to the right (clockwise). * @return Eigen::Vector3f The magnetic field vector in the body frame, in Gauss. */ -Eigen::Vector3f calculateMagneticVector(float lat, float lon, float alt, float roll, float pitch, float yaw) { +Eigen::Vector3f MAVLinkManager::calculateMagneticVector(float lat, float lon, float alt, float roll, float pitch, float yaw) { // Convert latitude and longitude from degrees to radians float latRad = lat * M_PI / 180.0; float lonRad = lon * M_PI / 180.0; @@ -83,7 +74,41 @@ Eigen::Vector3f calculateMagneticVector(float lat, float lon, float alt, float r } - +/** + * @brief Sends the HIL_SENSOR MAVLink message. + * + * This function constructs and sends the HIL_SENSOR message which contains simulated + * sensor readings for Hardware-In-the-Loop (HIL) simulation. It checks if a connection + * is established before sending the message. The function gathers data from various + * simulation sources, applies necessary conversions, and populates the HIL_SENSOR message. + * + * The following sensor data is populated: + * - Acceleration + * - Gyroscope + * - Pressure + * - Magnetic Field + * - Temperature + * + * The fields_updated bitmask in the HIL_SENSOR message is set to indicate which + * sensor readings are being updated. For more details on the bitmask values, refer to: + * https://mavlink.io/en/messages/common.html#HIL_SENSOR_UPDATED_FLAGS + * + * @note Ensure that the ConnectionManager is initialized and a connection is established + * before calling this function. + * + * @warning This function should be called at appropriate simulation update rates to ensure + * timely delivery of sensor data. + * + * Usage: + * @code + * MAVLinkManager::sendHILSensor(); + * @endcode + * + * @see setAccelerationData + * @see setGyroData + * @see setPressureData + * @see setMagneticFieldData + */ void MAVLinkManager::sendHILSensor() { if (!ConnectionManager::isConnected()) return; @@ -93,75 +118,10 @@ void MAVLinkManager::sendHILSensor() { hil_sensor.time_usec = static_cast(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6); hil_sensor.id = uint8_t(0); - - const float DEG_TO_RAD = 3.14159265358979323846 / 180.0; // Conversion factor from degrees to radians - const float GRAVITY = 9.81; // Gravitational acceleration in m/s^2 - - // Get the accelerations from X-Plane's OGL frame - float ax_OGL = DataRefManager::getFloat("sim/flightmodel/position/local_ax"); - float ay_OGL = DataRefManager::getFloat("sim/flightmodel/position/local_ay"); - float az_OGL = DataRefManager::getFloat("sim/flightmodel/position/local_az"); - - - - - 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; - - - - 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") ; - - - // Get the Location - float latitude = DataRefManager::getFloat("sim/flightmodel/position/latitude"); - float longitude = DataRefManager::getFloat("sim/flightmodel/position/longitude"); - float altitude = DataRefManager::getFloat("sim/flightmodel/position/elevation"); - - // 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 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; - - // Get the heading, roll, and pitch - 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 - 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; - - // Generate random noise values for the magnetometer data - std::random_device rd_mag; - std::mt19937 gen_mag(rd_mag()); - std::normal_distribution noiseDistribution_mag(0.0f, 0.01f); // Adjust the standard deviation as needed - - - // Call the calculateMagneticVector function - Eigen::Vector3f B_body = calculateMagneticVector(latitude, longitude, altitude, roll_rad, pitch_rad, yaw_rad); - - // Set the magnetic field in the HIL_SENSOR message - hil_sensor.xmag = B_body(0) + noiseDistribution_mag(gen_mag); - hil_sensor.ymag = B_body(1) + noiseDistribution_mag(gen_mag); - hil_sensor.zmag = B_body(2) + noiseDistribution_mag(gen_mag); - - + setAccelerationData(hil_sensor); + setGyroData(hil_sensor); + setPressureData(hil_sensor); + setMagneticFieldData(hil_sensor); hil_sensor.temperature = DataRefManager::getFloat("sim/cockpit2/temperature/outside_air_temp_degc"); @@ -169,22 +129,19 @@ void MAVLinkManager::sendHILSensor() { // 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 << 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, 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; + fields_updated |= (1 << 9); // HIL_SENSOR_UPDATED_ABS_PRESSURE + fields_updated |= (1 << 11); // HIL_SENSOR_UPDATED_PRESSURE_ALT + fields_updated |= (1 << 12); // HIL_SENSOR_UPDATED_TEMPERATURE - // 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); @@ -194,52 +151,51 @@ void MAVLinkManager::sendHILSensor() { } +/** + * @brief Sends the HIL_GPS MAVLink message. + * + * This function constructs and sends the HIL_GPS message which contains simulated + * GPS readings for Hardware-In-the-Loop (HIL) simulation. It checks if a connection + * is established before sending the message. The function gathers data from the simulation + * environment, applies necessary conversions, and populates the HIL_GPS message. + * + * The following GPS data is populated: + * - Time + * - Fix type + * - Latitude and Longitude + * - Altitude + * - Horizontal and Vertical Dilution of Precision (HDOP & VDOP) + * - Number of visible satellites + * - Velocities in North, East, and Down directions + * - Ground speed + * - Course over ground + * - GPS ID and Yaw + * + * @note Ensure that the ConnectionManager is initialized and a connection is established + * before calling this function. + * + * @warning This function should be called at appropriate simulation update rates to ensure + * timely delivery of GPS data. + * + * Usage: + * @code + * MAVLinkManager::sendHILGPS(); + * @endcode + * + * @see setGPSLocationData + * @see setGPSVelocityData + */ 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, assuming we always have a 3D fix in the simulation environment - - 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: 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. - - 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; - - float roll_rad = DataRefManager::getFloat("sim/flightmodel/position/phi") * M_PI / 180.0; - float pitch_rad = DataRefManager::getFloat("sim/flightmodel/position/theta") * M_PI / 180.0; - float yaw_rad = DataRefManager::getFloat("sim/flightmodel/position/psi") * M_PI / 180.0; - - - float hpath = DataRefManager::getFloat(" sim/flightmodel/position/hpath"); - float vpath = DataRefManager::getFloat(" sim/flightmodel/position/vpath"); - float groundspeed = DataRefManager::getFloat(" sim/flightmodel/position/groundspeed"); - - - - - //local OGL is : - // X: East - // Y: Up - // Z: South - hil_gps.vn = -1*ogl_vz; - hil_gps.ve = ogl_vx; - hil_gps.vd = -1* ogl_vy; - hil_gps.vel = groundspeed*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.id = 0; // 0 indexed GPS ID for single GPS - hil_gps.yaw = DataRefManager::getFloat("sim/flightmodel/position/psi") * 100; + setGPSTimeAndFix(hil_gps); + setGPSPositionData(hil_gps); + setGPSAccuracyData(hil_gps); + setGPSVelocityData(hil_gps); + setGPSHeadingData(hil_gps); mavlink_msg_hil_gps_encode(1, 1, &msg, &hil_gps); uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; @@ -249,20 +205,39 @@ void MAVLinkManager::sendHILGPS() { - +/** + * @brief Sends the HIL state quaternion message. + * + * This function fetches the required data, populates the MAVLink HIL state + * quaternion message, and sends it. + */ void MAVLinkManager::sendHILStateQuaternion() { - if (!ConnectionManager::isConnected()) return; mavlink_message_t msg; mavlink_hil_state_quaternion_t hil_state; + populateHILStateQuaternion(hil_state); + + mavlink_msg_hil_state_quaternion_encode(1, 1, &msg, &hil_state); + sendData(msg); +} + +/** + * @brief Populates the HIL state quaternion structure. + * + * This function fetches the required data from the DataRefManager and sets + * the appropriate fields in the provided hil_state structure. + * + * @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); + std::vector 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]; + for (int i = 0; i < 4; ++i) { + hil_state.attitude_quaternion[i] = q[i]; + } hil_state.rollspeed = DataRefManager::getFloat("sim/flightmodel/position/Prad"); hil_state.pitchspeed = DataRefManager::getFloat("sim/flightmodel/position/Qrad"); @@ -270,27 +245,51 @@ void MAVLinkManager::sendHILStateQuaternion() { 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); - hil_state.vx = static_cast(DataRefManager::getFloat("sim/flightmodel/position/local_vx") * 100); - hil_state.vy = static_cast(DataRefManager::getFloat("sim/flightmodel/position/local_vy") * 100); - hil_state.vz = static_cast(DataRefManager::getFloat("sim/flightmodel/position/local_vz") * 100); + // Convert OGL local 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) 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); - hil_state.xacc = static_cast(DataRefManager::getFloat("sim/flightmodel/position/local_ax") * 1000 / 9.81); - hil_state.yacc = static_cast(DataRefManager::getFloat("sim/flightmodel/position/local_ay") * 1000 / 9.81); - hil_state.zacc = static_cast(DataRefManager::getFloat("sim/flightmodel/position/local_az") * 1000 / 9.81); + 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; +} - mavlink_msg_hil_state_quaternion_encode(1, 1, &msg, &hil_state); +/** + * @brief Sends the provided MAVLink message. + * + * This function encodes the provided MAVLink message into a buffer and sends it. + * + * @param msg The MAVLink message to be sent. + */ +void MAVLinkManager::sendData(const mavlink_message_t& msg) { uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; int len = mavlink_msg_to_send_buffer(buffer, &msg); - ConnectionManager::sendData(buffer, len); } + + + +/** + * @brief Sends the HIL RC Inputs to the MAVLink connection. + * + * This function fetches the required RC input data from the DataRefManager, + * maps the values to the appropriate MAVLink format, and sends the HIL RC + * Inputs message over the MAVLink connection. + */ void MAVLinkManager::sendHILRCInputs() { if (!ConnectionManager::isConnected()) return; @@ -298,14 +297,15 @@ void MAVLinkManager::sendHILRCInputs() { 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); - hil_rc_inputs.chan1_raw = static_cast(DataRefManager::mapChannelValue(DataRefManager::getFloat("sim/joystick/yoke_roll_ratio"),-1,+1, 1000, 2000)); - hil_rc_inputs.chan2_raw = static_cast(DataRefManager::mapChannelValue(DataRefManager::getFloat("sim/joystick/yoke_pitch_ratio"), -1, +1, 1000, 2000)); - hil_rc_inputs.chan3_raw = static_cast(DataRefManager::mapChannelValue(DataRefManager::getFloat("sim/cockpit2/engine/actuators/throttle_ratio_all"),0, +1, 1000, 2000)); - hil_rc_inputs.chan4_raw = static_cast(DataRefManager::mapChannelValue(DataRefManager::getFloat("sim/joystick/yoke_heading_ratio"), -1, +1, 1000, 2000)); - hil_rc_inputs.chan5_raw = static_cast(DataRefManager::mapChannelValue(0, -1, +1, 1000, 2000)); + 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 - hil_rc_inputs.rssi = 255; // Set the RSSI field to a constant value if not available + constexpr uint8_t RSSI_MAX_VALUE = 255; + hil_rc_inputs.rssi = RSSI_MAX_VALUE; // Set the RSSI field to its maximum value mavlink_msg_hil_rc_inputs_raw_encode(1, 1, &msg, &hil_rc_inputs); @@ -315,51 +315,209 @@ void MAVLinkManager::sendHILRCInputs() { ConnectionManager::sendData(buffer, len); } +/** + * @brief Maps the RC channel value to the MAVLink format. + * + * This helper function maps an RC channel value from its original range + * to the MAVLink expected range of [1000, 2000]. + * + * @param value The original value of the RC channel. + * @param min The minimum value of the original range. + * @param max The maximum value of the original range. + * @return The mapped value in the MAVLink format. + */ +uint16_t MAVLinkManager::mapRCChannel(float value, float min, float max) { + return static_cast(DataRefManager::mapChannelValue(value, min, max, 1000, 2000)); +} + + +/** + * @brief Receives and processes HIL actuator control messages. + * + * This function parses the received buffer for MAVLink messages and processes + * the HIL actuator control messages by extracting and storing the relevant data. + * + * @param buffer Pointer to the received data buffer. + * @param size Size of the received data buffer. + */ void MAVLinkManager::receiveHILActuatorControls(uint8_t* buffer, int size) { if (!ConnectionManager::isConnected()) return; - // Parse the received data mavlink_message_t msg; mavlink_status_t status; for (int i = 0; i < size; ++i) { if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status)) { - // Handle the received message - switch (msg.msgid) { - case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: { - mavlink_hil_actuator_controls_t hil_actuator_controls; - mavlink_msg_hil_actuator_controls_decode(&msg, &hil_actuator_controls); - - // Extract the relevant information from the message - uint64_t timestamp = hil_actuator_controls.time_usec; - float controls[16]; - for (int i = 0; i < 16; ++i) { - controls[i] = hil_actuator_controls.controls[i]; - } - uint8_t mode = hil_actuator_controls.mode; - uint64_t flags = hil_actuator_controls.flags; - - // Store the received data in a suitable data structure or variable - // For example, you can store it in a class member variable or pass it to another function for further processing - - // Store the received data in the member variable - MAVLinkManager::hilActuatorControlsData.timestamp = timestamp; - for (int i = 0; i < 16; ++i) { - MAVLinkManager::hilActuatorControlsData.controls[i] = controls[i]; - } - MAVLinkManager::hilActuatorControlsData.mode = mode; - MAVLinkManager::hilActuatorControlsData.flags = flags; - - // Example: Pass the received data to another function for further processing - - break; - } - // Handle other MAVLink message types if needed - - default: - // Handle unrecognized message types if needed - break; - } + handleReceivedMessage(msg); } } } + +/** + * @brief Handles the received MAVLink message. + * + * This function checks the type of the received MAVLink message and processes + * it accordingly. + * + * @param msg The received MAVLink message. + */ +void MAVLinkManager::handleReceivedMessage(const mavlink_message_t& msg) { + switch (msg.msgid) { + case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: + processHILActuatorControlsMessage(msg); + break; + // Handle other MAVLink message types if needed + default: + // Handle unrecognized message types if needed + break; + } +} + +/** + * @brief Processes the HIL actuator control message. + * + * This function decodes the HIL actuator control message, extracts the relevant + * information, and stores it in the hilActuatorControlsData member variable. + * + * @param msg The received HIL actuator control MAVLink message. + */ +void MAVLinkManager::processHILActuatorControlsMessage(const mavlink_message_t& msg) { + mavlink_hil_actuator_controls_t hil_actuator_controls; + mavlink_msg_hil_actuator_controls_decode(&msg, &hil_actuator_controls); + + MAVLinkManager::hilActuatorControlsData.timestamp = hil_actuator_controls.time_usec; + for (int i = 0; i < 16; ++i) { + MAVLinkManager::hilActuatorControlsData.controls[i] = hil_actuator_controls.controls[i]; + } + MAVLinkManager::hilActuatorControlsData.mode = hil_actuator_controls.mode; + MAVLinkManager::hilActuatorControlsData.flags = hil_actuator_controls.flags; +} + + + +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; +} + +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"); +} + +void MAVLinkManager::setPressureData(mavlink_hil_sensor_t& hil_sensor) { + float basePressure = DataRefManager::getFloat("sim/weather/barometer_current_inhg") * 33.8639; + float pressureNoise = noiseDistribution(gen); + hil_sensor.abs_pressure = basePressure + pressureNoise; + hil_sensor.pressure_alt = DataRefManager::getDouble("sim/flightmodel2/position/pressure_altitude") * 0.3048; + hil_sensor.diff_pressure = 0; +} + +void MAVLinkManager::setMagneticFieldData(mavlink_hil_sensor_t& hil_sensor) { + // Get the heading, roll, and pitch + 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 + 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 + 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); + + // Generate random noise values for the magnetometer data + 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; +} + +/** + * @brief Sets the time and fix type data for the HIL_GPS message. + * + * @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 = 3; // Assuming always a 3D fix in the simulation environment +} + +/** + * @brief Sets the position data for the HIL_GPS message. + * + * @param hil_gps Reference to the mavlink_hil_gps_t structure to populate. + */ +void MAVLinkManager::setGPSPositionData(mavlink_hil_gps_t& hil_gps) { + hil_gps.lat = static_cast(DataRefManager::getFloat("sim/flightmodel/position/latitude") * 1e7); + hil_gps.lon = static_cast(DataRefManager::getFloat("sim/flightmodel/position/longitude") * 1e7); + hil_gps.alt = static_cast(DataRefManager::getFloat("sim/flightmodel/position/elevation") * 1e3); +} + +/** + * @brief Sets the accuracy data for the HIL_GPS message. + * + * @param hil_gps Reference to the mavlink_hil_gps_t structure to populate. + */ +void MAVLinkManager::setGPSAccuracyData(mavlink_hil_gps_t& hil_gps) { + hil_gps.eph = 20; // Assuming high accuracy due to simulation environment + hil_gps.epv = 20; + hil_gps.satellites_visible = 12; // Assuming 12 satellites visible in good conditions +} + +/** + * @brief Sets the velocity data for the HIL_GPS message. + * + * This function extracts the velocity data from the simulation environment in the OGL (OpenGL) coordinate system + * and converts it to the NED (North-East-Down) coordinate system for the HIL_GPS message. + * + * @param hil_gps Reference to the mavlink_hil_gps_t structure to populate. + */ +void MAVLinkManager::setGPSVelocityData(mavlink_hil_gps_t& hil_gps) { + 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; + + // Coordinate Transformation: + // The local OGL (OpenGL) coordinate system in the simulation environment is defined as: + // X: East + // Y: Up + // Z: South + // + // The NED (North-East-Down) coordinate system, commonly used in aviation, is defined as: + // X: North + // Y: East + // Z: Down + // + // The transformation from OGL to NED for velocities is: + // NED North (X) = -OGL South (Z) + // NED East (Y) = OGL East (X) + // NED Down (Z) = -OGL Up (Y) + + hil_gps.vn = -1 * ogl_vz; + hil_gps.ve = ogl_vx; + hil_gps.vd = -1 * ogl_vy; + hil_gps.vel = DataRefManager::getFloat("sim/flightmodel/position/groundspeed") * 100; +} + +/** + * @brief Sets the heading data for the HIL_GPS message. + * + * @param hil_gps Reference to the mavlink_hil_gps_t structure to populate. + */ +void MAVLinkManager::setGPSHeadingData(mavlink_hil_gps_t& hil_gps) { + hil_gps.cog = static_cast(DataRefManager::getFloat("sim/cockpit2/gauges/indicators/ground_track_mag_copilot") * 100); + hil_gps.yaw = DataRefManager::getFloat("sim/flightmodel/position/mag_psi") * 100; +} \ No newline at end of file diff --git a/MAVLinkManager.h b/MAVLinkManager.h index 06293d2..77d4c0e 100644 --- a/MAVLinkManager.h +++ b/MAVLinkManager.h @@ -1,4 +1,12 @@ #include +#include +#include +#include +#include "mavlink/c_library_v2/common/mavlink.h" +#include "XYZgeomag/src/XYZgeomag.hpp" +#include +#include // for trigonometric functions and M_PI + class MAVLinkManager { public: @@ -6,7 +14,12 @@ class MAVLinkManager { static void sendHILGPS(); static void sendHILStateQuaternion(); static void sendHILRCInputs(); + static void setAccelerationData(mavlink_hil_sensor_t& hil_sensor); + static void setGyroData(mavlink_hil_sensor_t& hil_sensor); + static void setPressureData(mavlink_hil_sensor_t& hil_sensor); + static void setMagneticFieldData(mavlink_hil_sensor_t& hil_sensor); static void receiveHILActuatorControls(uint8_t* buffer, int size); + static Eigen::Vector3f calculateMagneticVector(float latitude, float longitude, float altitude, float roll_rad, float pitch_rad, float yaw_rad); struct HILActuatorControlsData { uint64_t timestamp; @@ -14,6 +27,23 @@ class MAVLinkManager { uint8_t mode; uint64_t flags; }; - static HILActuatorControlsData hilActuatorControlsData; // Public variable to store the received data + + +private: + static std::random_device rd; + static std::mt19937 gen; + static std::normal_distribution noiseDistribution; + static std::normal_distribution noiseDistribution_mag; + static void setGPSTimeAndFix(mavlink_hil_gps_t& hil_gps); + static void setGPSPositionData(mavlink_hil_gps_t& hil_gps); + static void setGPSAccuracyData(mavlink_hil_gps_t& hil_gps); + static void setGPSVelocityData(mavlink_hil_gps_t& hil_gps); + static void setGPSHeadingData(mavlink_hil_gps_t& hil_gps); + static void handleReceivedMessage(const mavlink_message_t& msg); + static void processHILActuatorControlsMessage(const mavlink_message_t& msg); + static void populateHILStateQuaternion(mavlink_hil_state_quaternion_t& hil_state); + static void sendData(const mavlink_message_t& msg); + static uint16_t mapRCChannel(float value, float min, float max); + }; diff --git a/px4xplane.cpp b/px4xplane.cpp index 6ba3627..0d18794 100644 --- a/px4xplane.cpp +++ b/px4xplane.cpp @@ -105,9 +105,6 @@ PLUGIN_API int XPluginStart( - // TODO: Setup the TCP connection here - // int sock = socket(AF_INET, SOCK_STREAM, 0); - // ... return 1; } @@ -280,7 +277,7 @@ float MyFlightLoopCallback(float inElapsedSinceLastCall, float inElapsedTimeSinc PLUGIN_API void XPluginStop(void) { // Unregister the flight loop callback - if (ConnectionManager::isConnected) { + if (ConnectionManager::isConnected()) { toggleEnable(); } XPLMUnregisterFlightLoopCallback(MyFlightLoopCallback, NULL);