Skip to content

Commit

Permalink
all workin except magnetomter
Browse files Browse the repository at this point in the history
  • Loading branch information
@alireza787b
alireza787b committed Oct 15, 2023
1 parent 9db6858 commit 3bee034
Show file tree
Hide file tree
Showing 5 changed files with 127 additions and 24 deletions.
57 changes: 47 additions & 10 deletions DataRefManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,8 @@
#include <string>
#include "XPLMDisplay.h"
#include "XPLMGraphics.h"

#include <vector>
#include<cmath>


std::vector<DataRefItem> DataRefManager::dataRefs = {
Expand All @@ -14,6 +15,7 @@ std::vector<DataRefItem> DataRefManager::dataRefs = {
{"Position", "Longitude", "sim/flightmodel/position/longitude", "deg", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Position", "Longitude", "sim/flightmodel/position/longitude", "deg", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Position", "Altitude", "sim/flightmodel/position/elevation", "m", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Position", "Ground Speed", "sim/flightmodel/position/groundspeed", "m/s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Position", "Course (Track)", "sim/cockpit2/gauges/indicators/ground_track_mag_copilot", "deg", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},

// Attitude Information
Expand All @@ -22,40 +24,46 @@ std::vector<DataRefItem> DataRefManager::dataRefs = {
{"Attitude", "Heading", "sim/flightmodel/position/psi", "deg", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Attitude", "Quaternion", "sim/flightmodel/position/q", "", 1, DRT_FLOAT_ARRAY, std::function<std::vector<float>(const char*)>(getFloatArray)},

// Acceleration Information
{"Acceleration (NED)", "X-Acc", "sim/flightmodel/position/local_ax", "m/s^2", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Acceleration (NED)", "Y-Acc", "sim/flightmodel/position/local_ay", "m/s^2", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Acceleration (NED)", "Z-Acc", "sim/flightmodel/position/local_az", "m/s^2", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
//// Acceleration Information
//{"Acceleration (NED)", "X-Acc", "sim/flightmodel/position/local_ax", "m/s^2", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
//{"Acceleration (NED)", "Y-Acc", "sim/flightmodel/position/local_ay", "m/s^2", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
//{"Acceleration (NED)", "Z-Acc", "sim/flightmodel/position/local_az", "m/s^2", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},

// Acceleration Information
{"Acceleration (BODY)", "X-Acc", "sim/flightmodel/forces/g_axil", "m/s^2", g_earth, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Acceleration (BODY)", "Y-Acc", "sim/flightmodel/forces/g_side", "m/s^2", g_earth, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Acceleration (BODY)", "Z-Acc", "sim/flightmodel/forces/g_nrml", "m/s^2", g_earth, DRT_FLOAT, std::function<float(const char*)>(getFloat)},

// Ground Speed Information
{"Ground Speed", "Vx", "sim/flightmodel/position/local_vx", "m/s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
/*{"Ground Speed", "Vx", "sim/flightmodel/position/local_vx", "m/s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Ground Speed", "Vy", "sim/flightmodel/position/local_vy", "m/s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Ground Speed", "Vz", "sim/flightmodel/position/local_vz", "m/s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Ground Speed", "Vz", "sim/flightmodel/position/local_vz", "m/s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},*/

// Airspeed Information
{"Airspeed", "IAS", "sim/flightmodel/position/indicated_airspeed", "m/s", 0.514444, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Airspeed", "TAS", "sim/flightmodel/position/true_airspeed", "m/s", 0.514444, DRT_FLOAT, std::function<float(const char*)>(getFloat)},

// Pressure Information
{"Pressure", "Pressure Altitude", "sim/flightmodel2/position/pressure_altitude", "m", 0.3048, DRT_DOUBLE, std::function<double(const char*)>(getDouble)},
{"Pressure", "Absolute Pressure", "sim/weather/barometer_current_inhg", "hPa", 33.8639, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
//{"Pressure", "Pressure Altitude", "sim/flightmodel2/position/pressure_altitude", "m", 0.3048, DRT_DOUBLE, std::function<double(const char*)>(getDouble)},

// Angular Velocities
{"Angular Velocity", "P", "sim/flightmodel/position/Prad", "rad/s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Angular Velocity", "Q", "sim/flightmodel/position/Qrad", "rad/s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Angular Velocity", "R", "sim/flightmodel/position/Rrad", "rad/s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},

// Temperature
{"Temperature", "Outside Air Temp", "sim/cockpit2/temperature/outside_air_temp_degc", "degC", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Sensors", "Outside Air Temp", "sim/cockpit2/temperature/outside_air_temp_degc", "degC", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Sensors", "Absolute Pressure", "sim/weather/barometer_current_inhg", "hPa", 33.8639, DRT_FLOAT, std::function<float(const char*)>(getFloat)},

// Time Information
{"Time", "Total Flight Time", "sim/time/total_flight_time_sec", "s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"Time", "Zulu Time", "sim/time/zulu_time_sec", "s", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},

// RC Information
{"RC Information", "Aileron", "sim/joystick/yoke_roll_ratio", "", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"RC Information", "Elevator", "sim/joystick/yoke_pitch_ratio", "", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"RC Information", "Throttle", "sim/cockpit2/engine/actuators/throttle_ratio_all", "", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
{"RC Information", "Rudder", "sim/joystick/yoke_heading_ratio", "", 1, DRT_FLOAT, std::function<float(const char*)>(getFloat)},
};


Expand Down Expand Up @@ -134,3 +142,32 @@ std::string DataRefManager::arrayToString(const std::vector<float>& 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<float, float, float> 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);
}

9 changes: 6 additions & 3 deletions DataRefManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,8 @@
#include "XPLMDataAccess.h"
#include "XPLMDisplay.h"
#include <string>

#include<cmath>
#include<vector>

enum DataRefType {
DRT_FLOAT,
Expand Down Expand Up @@ -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<float>& 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<float, float, float> convertOGLtoNED(float ogl_vx, float ogl_vy, float ogl_vz, float roll_rad, float pitch_rad, float yaw_rad);

};

82 changes: 71 additions & 11 deletions MAVLinkManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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");

Expand All @@ -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
Expand Down Expand Up @@ -79,20 +105,31 @@ void MAVLinkManager::sendHILGPS() {
hil_gps.lon = static_cast<int32_t>(DataRefManager::getFloat("sim/flightmodel/position/longitude") * 1e7); // converted to degE7

hil_gps.alt = static_cast<int32_t>(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<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_vx") * 100); // converted to cm/s
hil_gps.ve = static_cast<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/local_vy") * 100); // converted to cm/s
hil_gps.vd = static_cast<int16_t>(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<int16_t>(DataRefManager::getFloat("sim/flightmodel/position/groundspeed") * 100); // converted to cm/s

hil_gps.cog = static_cast<uint16_t>(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];
Expand Down Expand Up @@ -141,4 +178,27 @@ void MAVLinkManager::sendHILStateQuaternion() {
int len = mavlink_msg_to_send_buffer(buffer, &msg);

ConnectionManager::sendData(buffer, len);
}
}
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<uint64_t>(DataRefManager::getFloat("sim/time/total_flight_time_sec") * 1e6);
hil_rc_inputs.chan1_raw = static_cast<uint16_t>(DataRefManager::mapChannelValue(DataRefManager::getFloat("sim/joystick/yoke_roll_ratio"),-1,+1, 1000, 2000));
hil_rc_inputs.chan2_raw = static_cast<uint16_t>(DataRefManager::mapChannelValue(DataRefManager::getFloat("sim/joystick/yoke_pitch_ratio"), -1, +1, 1000, 2000));
hil_rc_inputs.chan3_raw = static_cast<uint16_t>(DataRefManager::mapChannelValue(DataRefManager::getFloat("sim/cockpit2/engine/actuators/throttle_ratio_all"),0, +1, 1000, 2000));
hil_rc_inputs.chan4_raw = static_cast<uint16_t>(DataRefManager::mapChannelValue(DataRefManager::getFloat("sim/joystick/yoke_heading_ratio"), -1, +1, 1000, 2000));
hil_rc_inputs.chan4_raw = static_cast<uint16_t>(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);
}
2 changes: 2 additions & 0 deletions MAVLinkManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,5 +6,7 @@ class MAVLinkManager {
static void sendHILSensor();
static void sendHILGPS();
static void sendHILStateQuaternion();
static void sendHILRCInputs();


};
1 change: 1 addition & 0 deletions px4xplane.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -246,6 +246,7 @@ float MyFlightLoopCallback(float inElapsedSinceLastCall, float inElapsedTimeSinc
MAVLinkManager::sendHILSensor();
MAVLinkManager::sendHILGPS();
MAVLinkManager::sendHILStateQuaternion();
MAVLinkManager::sendHILRCInputs();


//ConnectionManager::receiveData();
Expand Down

0 comments on commit 3bee034

Please sign in to comment.