From b6109394aec31f738553ed2abfdcb1876e9e8365 Mon Sep 17 00:00:00 2001 From: Alireza Ghaderi Date: Wed, 29 Nov 2023 17:12:02 +0000 Subject: [PATCH] working fixedWing --- config/config.ini | 16 +++++++++------- src/ConfigManager.cpp | 2 +- src/DataRefManager.cpp | 13 +++++-------- src/MAVLinkManager.cpp | 28 +++++++++++++++------------- src/px4xplane.cpp | 6 +++--- 5 files changed, 33 insertions(+), 32 deletions(-) diff --git a/config/config.ini b/config/config.ini index 30a3bcf..0cec890 100644 --- a/config/config.ini +++ b/config/config.ini @@ -46,17 +46,19 @@ motor4 = 4 ; PX4 Motor 4 mapped to X-Plane Motor 4 # - Array Indices: The indices for array data types (use 0 for non-array types). # - Range: The range of values, specified as [min max] without a comma. -# Aileron Left (Servo 1) -# Dataref: sim/flightmodel/controls/wing1l_ail1def +# Aileron Left (Servo 1) (Usually seen from front of plane in X-Plane) +# Dataref: sim/flightmodel/controls/wing1l_ail1def is for visual only +# Dataref: sim/flightmodel/controls/lail1def # Type: float # Range: [-20 15] (degrees of deflection) -channel0 = sim/flightmodel/controls/wing1l_ail1def, float, 0, [-20 15] +channel0 = sim/flightmodel/controls/wing2l_ail1def , float, 0, [-20 15] -# Aileron Right (Servo 2) -# Dataref: sim/flightmodel/controls/wing1r_ail1def +# Aileron Right (Servo 2) (Usually seen from front of plane in X-Plane) +# Dataref: sim/flightmodel/controls/wing1r_ail1def is for visual only +# Dataref: sim/flightmodel/controls/rail1def # Type: float # Range: [-20 15] (degrees of deflection) -channel1 = sim/flightmodel/controls/wing1r_ail1def, float, 0, [-20 15] +channel1 = sim/flightmodel/controls/wing2r_ail1def , float, 0, [-20 15] # Elevator (Servo 3) # Dataref: sim/flightmodel/controls/hstab1_elv1def @@ -65,7 +67,7 @@ channel1 = sim/flightmodel/controls/wing1r_ail1def, float, 0, [-20 15] channel2 = sim/flightmodel/controls/hstab1_elv1def, float, 0, [-20 15] # Throttle (Motor 1) -# Dataref: sim/flightmodel/engine/ENGN_thro_use (array indices (+1) is the engine number) +# Dataref: sim/flightmodel/engine/ENGN_thro_use ([array indices + 1] is the engine number) # Type: floatArray # Range: [-1 1] (idle (0) to full throttle Foreward (+1) ) channel3 = sim/flightmodel/engine/ENGN_thro_use, floatArray, [0], [-1 1] diff --git a/src/ConfigManager.cpp b/src/ConfigManager.cpp index 80d647a..1829c81 100644 --- a/src/ConfigManager.cpp +++ b/src/ConfigManager.cpp @@ -185,7 +185,7 @@ void ConfigManager::parseMultirotorConfig(CSimpleIniA& ini) { void ConfigManager::parseFixedWingConfig(CSimpleIniA& ini) { XPLMDebugString("px4xplane: Starting to parse FixedWing configuration.\n"); - for (int channel = 0; channel < 16; ++channel) { + for (int channel = 0; channel < 16; channel++) { std::string key = "channel" + std::to_string(channel); std::string value = ini.GetValue("FixedWing", key.c_str(), ""); diff --git a/src/DataRefManager.cpp b/src/DataRefManager.cpp index 7717aa1..0231c34 100644 --- a/src/DataRefManager.cpp +++ b/src/DataRefManager.cpp @@ -134,9 +134,9 @@ Eigen::Vector3f DataRefManager::updateEarthMagneticFieldNED(float lat, float lon float lonRad = lon * M_PI / 180.0; // Log the input values - char log[256]; + /*char log[256]; sprintf(log, "updateEarthMagneticFieldNED called with lat: %f, lon: %f, alt: %f\n", lat, lon, alt); - XPLMDebugString(log); + XPLMDebugString(log);*/ geomag::Vector position = geomag::geodetic2ecef(lat, lon, alt); geomag::Vector mag_field = geomag::GeoMag(2022.5, position, geomag::WMM2020); @@ -145,8 +145,8 @@ Eigen::Vector3f DataRefManager::updateEarthMagneticFieldNED(float lat, float lon earthMagneticFieldNED = Eigen::Vector3f(nedElements.north, nedElements.east, nedElements.down); // Log the calculated magnetic field - sprintf(log, "Calculated magnetic field NED: north: %f, east: %f, down: %f\n", nedElements.north, nedElements.east, nedElements.down); - XPLMDebugString(log); + /*sprintf(log, "Calculated magnetic field NED: north: %f, east: %f, down: %f\n", nedElements.north, nedElements.east, nedElements.down); + XPLMDebugString(log)*/; earthMagneticFieldNED *= 0.00001; // Convert from nT to Gauss @@ -390,10 +390,7 @@ void DataRefManager::overrideActuators_multirotor() { if (xplaneMotor != -1) { // Check if mapping exists correctedControls.controls[xplaneMotor - 1] = hilControls.controls[i]; - // Debug print - char debugMsg[100]; - snprintf(debugMsg, sizeof(debugMsg), "PX4 Motor %d -> X-Plane Motor %d\n", i + 1, xplaneMotor); - XPLMDebugString(debugMsg); + } } diff --git a/src/MAVLinkManager.cpp b/src/MAVLinkManager.cpp index 4ecc1ee..0bbeecf 100644 --- a/src/MAVLinkManager.cpp +++ b/src/MAVLinkManager.cpp @@ -478,7 +478,7 @@ void MAVLinkManager::setMagneticFieldData(mavlink_hil_sensor_t& hil_sensor) { */ 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 + hil_gps.fix_type = static_cast(3); // Assuming always a 3D fix in the simulation environment } /** @@ -498,9 +498,9 @@ void MAVLinkManager::setGPSPositionData(mavlink_hil_gps_t& hil_gps) { * @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 + hil_gps.eph = static_cast(20); // Assuming high accuracy due to simulation environment + hil_gps.epv = static_cast(20); + hil_gps.satellites_visible = static_cast(14);; // Assuming 12 satellites visible in good conditions } /** @@ -512,9 +512,9 @@ void MAVLinkManager::setGPSAccuracyData(mavlink_hil_gps_t& hil_gps) { * @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; + int16_t ogl_vx = DataRefManager::getFloat("sim/flightmodel/position/local_vx") * 100; + int16_t ogl_vy = DataRefManager::getFloat("sim/flightmodel/position/local_vy") * 100; + int16_t 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: @@ -532,10 +532,10 @@ void MAVLinkManager::setGPSVelocityData(mavlink_hil_gps_t& hil_gps) { // 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; + hil_gps.vn = static_cast ( - 1 * ogl_vz); + hil_gps.ve = static_cast(ogl_vx); + hil_gps.vd = static_cast(-1 * ogl_vy); + hil_gps.vel = static_cast(DataRefManager::getFloat("sim/flightmodel/position/groundspeed") * 100); } /** @@ -544,6 +544,8 @@ void MAVLinkManager::setGPSVelocityData(mavlink_hil_gps_t& hil_gps) { * @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; + uint16_t cog = static_cast(DataRefManager::getFloat("sim/cockpit2/gauges/indicators/ground_track_mag_copilot") * 100); + hil_gps.cog = (cog == 0) ? 360 : cog; + uint16_t yaw = static_cast(DataRefManager::getFloat("sim/flightmodel/position/psi") * 100); + hil_gps.yaw = (yaw == 0) ? 360 : yaw; } \ No newline at end of file diff --git a/src/px4xplane.cpp b/src/px4xplane.cpp index f1b91e9..f4ec4bb 100644 --- a/src/px4xplane.cpp +++ b/src/px4xplane.cpp @@ -296,8 +296,8 @@ float MyFlightLoopCallback(float inElapsedSinceLastCall, float inElapsedTimeSinc // Call MAVLinkManager::sendHILSensor() to send HIL_SENSOR data MAVLinkManager::sendHILSensor(uint8_t(0)); MAVLinkManager::sendHILGPS(); - MAVLinkManager::sendHILStateQuaternion(); - MAVLinkManager::sendHILRCInputs(); + //MAVLinkManager::sendHILStateQuaternion(); + //MAVLinkManager::sendHILRCInputs(); ConnectionManager::receiveData(); @@ -305,7 +305,7 @@ float MyFlightLoopCallback(float inElapsedSinceLastCall, float inElapsedTimeSinc if (ConfigManager::getConfigTypeCode() == 1) { // Multirotor configuration DataRefManager::overrideActuators_multirotor(); - } else if (ConfigManager::getConfigTypeCode() == 2) { + } else { // Fixed-wing configuration DataRefManager::overrideActuators_fixedwing(); }