From 3bee0341d7845d96b989b8de57148fb3189f708f Mon Sep 17 00:00:00 2001 From: Alireza Ghaderi Date: Sun, 15 Oct 2023 09:53:17 +0000 Subject: [PATCH] all workin except magnetomter --- DataRefManager.cpp | 57 ++++++++++++++++++++++++++------ DataRefManager.h | 9 +++-- MAVLinkManager.cpp | 82 +++++++++++++++++++++++++++++++++++++++------- MAVLinkManager.h | 2 ++ px4xplane.cpp | 1 + 5 files changed, 127 insertions(+), 24 deletions(-) diff --git a/DataRefManager.cpp b/DataRefManager.cpp index a72f05d..b716d8d 100644 --- a/DataRefManager.cpp +++ b/DataRefManager.cpp @@ -5,7 +5,8 @@ #include #include "XPLMDisplay.h" #include "XPLMGraphics.h" - +#include +#include std::vector DataRefManager::dataRefs = { @@ -14,6 +15,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", "Ground Speed", "sim/flightmodel/position/groundspeed", "m/s", 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 @@ -22,10 +24,10 @@ std::vector DataRefManager::dataRefs = { {"Attitude", "Heading", "sim/flightmodel/position/psi", "deg", 1, DRT_FLOAT, std::function(getFloat)}, {"Attitude", "Quaternion", "sim/flightmodel/position/q", "", 1, DRT_FLOAT_ARRAY, std::function(const char*)>(getFloatArray)}, - // Acceleration Information - {"Acceleration (NED)", "X-Acc", "sim/flightmodel/position/local_ax", "m/s^2", 1, DRT_FLOAT, std::function(getFloat)}, - {"Acceleration (NED)", "Y-Acc", "sim/flightmodel/position/local_ay", "m/s^2", 1, DRT_FLOAT, std::function(getFloat)}, - {"Acceleration (NED)", "Z-Acc", "sim/flightmodel/position/local_az", "m/s^2", 1, DRT_FLOAT, std::function(getFloat)}, + //// Acceleration Information + //{"Acceleration (NED)", "X-Acc", "sim/flightmodel/position/local_ax", "m/s^2", 1, DRT_FLOAT, std::function(getFloat)}, + //{"Acceleration (NED)", "Y-Acc", "sim/flightmodel/position/local_ay", "m/s^2", 1, DRT_FLOAT, std::function(getFloat)}, + //{"Acceleration (NED)", "Z-Acc", "sim/flightmodel/position/local_az", "m/s^2", 1, DRT_FLOAT, std::function(getFloat)}, // Acceleration Information {"Acceleration (BODY)", "X-Acc", "sim/flightmodel/forces/g_axil", "m/s^2", g_earth, DRT_FLOAT, std::function(getFloat)}, @@ -33,17 +35,16 @@ std::vector DataRefManager::dataRefs = { {"Acceleration (BODY)", "Z-Acc", "sim/flightmodel/forces/g_nrml", "m/s^2", g_earth, DRT_FLOAT, std::function(getFloat)}, // Ground Speed Information - {"Ground Speed", "Vx", "sim/flightmodel/position/local_vx", "m/s", 1, DRT_FLOAT, std::function(getFloat)}, + /*{"Ground Speed", "Vx", "sim/flightmodel/position/local_vx", "m/s", 1, DRT_FLOAT, std::function(getFloat)}, {"Ground Speed", "Vy", "sim/flightmodel/position/local_vy", "m/s", 1, DRT_FLOAT, std::function(getFloat)}, - {"Ground Speed", "Vz", "sim/flightmodel/position/local_vz", "m/s", 1, DRT_FLOAT, std::function(getFloat)}, + {"Ground Speed", "Vz", "sim/flightmodel/position/local_vz", "m/s", 1, DRT_FLOAT, std::function(getFloat)},*/ // Airspeed Information {"Airspeed", "IAS", "sim/flightmodel/position/indicated_airspeed", "m/s", 0.514444, DRT_FLOAT, std::function(getFloat)}, {"Airspeed", "TAS", "sim/flightmodel/position/true_airspeed", "m/s", 0.514444, DRT_FLOAT, std::function(getFloat)}, // Pressure Information - {"Pressure", "Pressure Altitude", "sim/flightmodel2/position/pressure_altitude", "m", 0.3048, DRT_DOUBLE, std::function(getDouble)}, - {"Pressure", "Absolute Pressure", "sim/weather/barometer_current_inhg", "hPa", 33.8639, DRT_FLOAT, std::function(getFloat)}, + //{"Pressure", "Pressure Altitude", "sim/flightmodel2/position/pressure_altitude", "m", 0.3048, DRT_DOUBLE, std::function(getDouble)}, // Angular Velocities {"Angular Velocity", "P", "sim/flightmodel/position/Prad", "rad/s", 1, DRT_FLOAT, std::function(getFloat)}, @@ -51,11 +52,18 @@ std::vector DataRefManager::dataRefs = { {"Angular Velocity", "R", "sim/flightmodel/position/Rrad", "rad/s", 1, DRT_FLOAT, std::function(getFloat)}, // Temperature - {"Temperature", "Outside Air Temp", "sim/cockpit2/temperature/outside_air_temp_degc", "degC", 1, DRT_FLOAT, std::function(getFloat)}, + {"Sensors", "Outside Air Temp", "sim/cockpit2/temperature/outside_air_temp_degc", "degC", 1, DRT_FLOAT, std::function(getFloat)}, + {"Sensors", "Absolute Pressure", "sim/weather/barometer_current_inhg", "hPa", 33.8639, DRT_FLOAT, std::function(getFloat)}, // Time Information {"Time", "Total Flight Time", "sim/time/total_flight_time_sec", "s", 1, DRT_FLOAT, std::function(getFloat)}, {"Time", "Zulu Time", "sim/time/zulu_time_sec", "s", 1, DRT_FLOAT, std::function(getFloat)}, + + // RC Information + {"RC Information", "Aileron", "sim/joystick/yoke_roll_ratio", "", 1, DRT_FLOAT, std::function(getFloat)}, + {"RC Information", "Elevator", "sim/joystick/yoke_pitch_ratio", "", 1, DRT_FLOAT, std::function(getFloat)}, + {"RC Information", "Throttle", "sim/cockpit2/engine/actuators/throttle_ratio_all", "", 1, DRT_FLOAT, std::function(getFloat)}, + {"RC Information", "Rudder", "sim/joystick/yoke_heading_ratio", "", 1, DRT_FLOAT, std::function(getFloat)}, }; @@ -134,3 +142,32 @@ std::string DataRefManager::arrayToString(const std::vector& array) { result += "]"; return result; } +float DataRefManager::mapChannelValue(float value, float minInput, float maxInput, float minOutput, float maxOutput) { + // Map the input value from the input range to the output range + return ((value - minInput) / (maxInput - minInput)) * (maxOutput - minOutput) + minOutput; +} + +std::tuple DataRefManager::convertOGLtoNED(float ogl_vx, float ogl_vy, float ogl_vz, float roll_rad, float pitch_rad, float yaw_rad) { + // Calculate the rotation matrix from OGL to NED + // This is a simplified example and might not exactly match your requirements + float R[3][3]; + R[0][0] = cos(yaw_rad) * cos(pitch_rad); + R[0][1] = cos(yaw_rad) * sin(pitch_rad) * sin(roll_rad) - sin(yaw_rad) * cos(roll_rad); + R[0][2] = cos(yaw_rad) * sin(pitch_rad) * cos(roll_rad) + sin(yaw_rad) * sin(roll_rad); + + R[1][0] = sin(yaw_rad) * cos(pitch_rad); + R[1][1] = sin(yaw_rad) * sin(pitch_rad) * sin(roll_rad) + cos(yaw_rad) * cos(roll_rad); + R[1][2] = sin(yaw_rad) * sin(pitch_rad) * cos(roll_rad) - cos(yaw_rad) * sin(roll_rad); + + R[2][0] = -sin(pitch_rad); + R[2][1] = cos(pitch_rad) * sin(roll_rad); + R[2][2] = cos(pitch_rad) * cos(roll_rad); + + // Convert OGL velocities to NED velocities using the rotation matrix + float ned_vn = R[0][0] * ogl_vx + R[0][1] * ogl_vy + R[0][2] * ogl_vz; + float ned_ve = R[1][0] * ogl_vx + R[1][1] * ogl_vy + R[1][2] * ogl_vz; + float ned_vd = R[2][0] * ogl_vx + R[2][1] * ogl_vy + R[2][2] * ogl_vz; + + return std::make_tuple(ned_vn, ned_ve, ned_vd); +} + diff --git a/DataRefManager.h b/DataRefManager.h index b429c65..f121e5a 100644 --- a/DataRefManager.h +++ b/DataRefManager.h @@ -7,7 +7,8 @@ #include "XPLMDataAccess.h" #include "XPLMDisplay.h" #include - +#include +#include enum DataRefType { DRT_FLOAT, @@ -40,6 +41,8 @@ class DataRefManager { static int getInt(const char* dataRef); static double getDouble(const char* dataRef); static std::string arrayToString(const std::vector& array); // add this line - - // ... other functions for getting DataRefs ... + static float mapChannelValue(float value, float minInput, float maxInput, float minOutput, float maxOutput); + static std::tuple convertOGLtoNED(float ogl_vx, float ogl_vy, float ogl_vz, float roll_rad, float pitch_rad, float yaw_rad); + }; + diff --git a/MAVLinkManager.cpp b/MAVLinkManager.cpp index b418a9e..5a2cac7 100644 --- a/MAVLinkManager.cpp +++ b/MAVLinkManager.cpp @@ -15,7 +15,23 @@ void MAVLinkManager::sendHILSensor() { 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; + hil_sensor.zacc = DataRefManager::getFloat("sim/flightmodel/forces/g_nrml") * DataRefManager::g_earth*-1; + + /* float ogl_ax = DataRefManager::getFloat("sim/flightmodel/position/local_ax"); + float ogl_ay = DataRefManager::getFloat("sim/flightmodel/position/local_ay"); + float ogl_az = DataRefManager::getFloat("sim/flightmodel/position/local_az"); + + 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 ned_an, ned_ae, ned_ad; + std::tie(ned_an, ned_ae, ned_ad) = DataRefManager::convertOGLtoNED(ogl_ax, ogl_ay, ogl_az, roll_rad, pitch_rad, yaw_rad); + hil_sensor.xacc = ned_an; + hil_sensor.yacc = ned_ae; + hil_sensor.zacc = ned_ad;*/ + + hil_sensor.xgyro = DataRefManager::getFloat("sim/flightmodel/position/Prad"); hil_sensor.ygyro = DataRefManager::getFloat("sim/flightmodel/position/Qrad"); @@ -36,9 +52,16 @@ void MAVLinkManager::sendHILSensor() { 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; + + //create a siulated magnetomter here later + + // Add noise to the magnetic vector + /* hil_sensor.xmag = 0; + hil_sensor.ymag = 0; + hil_sensor.zmag = 0;*/ + + + hil_sensor.temperature = DataRefManager::getFloat("sim/cockpit2/temperature/outside_air_temp_degc"); @@ -52,6 +75,9 @@ void MAVLinkManager::sendHILSensor() { 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 << 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 @@ -79,20 +105,31 @@ void MAVLinkManager::sendHILGPS() { 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. - 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 + float ogl_vx = DataRefManager::getFloat("sim/flightmodel/position/local_vx"); + float ogl_vy = DataRefManager::getFloat("sim/flightmodel/position/local_vy"); + float ogl_vz = DataRefManager::getFloat("sim/flightmodel/position/local_vz"); + + 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 ned_vn, ned_ve, ned_vd; + std::tie(ned_vn, ned_ve, ned_vd) = DataRefManager::convertOGLtoNED(ogl_vx, ogl_vy, ogl_vz, roll_rad, pitch_rad, yaw_rad); + + + hil_gps.vn = ned_vn; + hil_gps.ve = ned_ve; + hil_gps.vd = ned_vd; + hil_gps.vel = static_cast(DataRefManager::getFloat("sim/flightmodel/position/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.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 + hil_gps.yaw = DataRefManager::getFloat("sim/flightmodel/position/psi") * 100; mavlink_msg_hil_gps_encode(1, 1, &msg, &hil_gps); uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; @@ -141,4 +178,27 @@ void MAVLinkManager::sendHILStateQuaternion() { int len = mavlink_msg_to_send_buffer(buffer, &msg); ConnectionManager::sendData(buffer, len); -} \ No newline at end of file +} +void MAVLinkManager::sendHILRCInputs() { + if (!ConnectionManager::isConnected()) return; + + 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); + 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.chan4_raw = static_cast(DataRefManager::mapChannelValue(0, -1, +1, 1000, 2000)); + // Map the remaining channels as needed + + hil_rc_inputs.rssi = 255; // Set the RSSI field to a constant value if not available + + mavlink_msg_hil_rc_inputs_raw_encode(1, 1, &msg, &hil_rc_inputs); + + 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 e98e564..65810f5 100644 --- a/MAVLinkManager.h +++ b/MAVLinkManager.h @@ -6,5 +6,7 @@ class MAVLinkManager { static void sendHILSensor(); static void sendHILGPS(); static void sendHILStateQuaternion(); + static void sendHILRCInputs(); + }; \ No newline at end of file diff --git a/px4xplane.cpp b/px4xplane.cpp index eb933af..0c721c7 100644 --- a/px4xplane.cpp +++ b/px4xplane.cpp @@ -246,6 +246,7 @@ float MyFlightLoopCallback(float inElapsedSinceLastCall, float inElapsedTimeSinc MAVLinkManager::sendHILSensor(); MAVLinkManager::sendHILGPS(); MAVLinkManager::sendHILStateQuaternion(); + MAVLinkManager::sendHILRCInputs(); //ConnectionManager::receiveData();