Skip to content

Commit

Permalink
Enable HIL simulation for jsbsim
Browse files Browse the repository at this point in the history 
This commit adds configuration options for running HITL simulation with the PX4 JSBSim bridge

The device path / baudrate can be passed with the command line flag -d and -b. The bridge is configured to be run by HITL when the -d is provided. the baudrate is configured to be 921600 by default 


Enable HITL on mavlink interface

This PR enables HITL on the mavlink interface
Handle gcs udp port in config file
  • Loading branch information
@Jaeyoung-Lim
Jaeyoung-Lim committed Nov 2, 2020
1 parent d185fab commit d12eef0
Show file tree
Hide file tree
Showing 7 changed files with 118 additions and 79 deletions.
16 changes: 10 additions & 6 deletions include/configuration_parser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,17 +42,14 @@

#include "common.h"

#include <memory>
#include <tinyxml.h>
#include <Eigen/Eigen>
#include <memory>

enum class ArgResult {
Success, Help, Error
};
enum class ArgResult { Success, Help, Error };

class ConfigurationParser {
public:

ConfigurationParser() = default;
~ConfigurationParser() = default;
bool ParseEnvironmentVariables();
Expand All @@ -62,7 +59,10 @@ class ConfigurationParser {
std::shared_ptr<TiXmlHandle> XmlHandle() { return _config; }
std::string getInitScriptPath() { return _init_script_path; }
std::string getModelName() { return _model_name; }
int getRealtimeFactor() { return _realtime_factor; }
int getRealtimeFactor() { return _realtime_factor; }
bool getSerialEnabled() { return _serial_enabled; }
int getBaudrate() { return _baudrate; }
std::string getDevice() { return _device; }
void setHeadless(bool headless) { _headless = headless; }
void setInitScriptPath(std::string path) { _init_script_path = path; }
static void PrintHelpMessage(char *argv[]);
Expand All @@ -75,4 +75,8 @@ class ConfigurationParser {
std::string _init_script_path;
std::string _model_name;
float _realtime_factor{1.0};
// HITL Configs
bool _serial_enabled{false};
std::string _device;
int _baudrate{921600};
};
3 changes: 2 additions & 1 deletion include/jsbsim_bridge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@
#include <chrono>

static constexpr int kDefaultSITLTcpPort = 4560;
static constexpr int kDefaultGCSPort = 14550;

class JSBSimBridge {
public:
Expand All @@ -67,7 +68,7 @@ class JSBSimBridge {

private:
bool SetFdmConfigs(ConfigurationParser &cfg);
bool SetMavlinkInterfaceConfigs(std::unique_ptr<MavlinkInterface> &interface, TiXmlHandle &config);
bool SetMavlinkInterfaceConfigs(std::unique_ptr<MavlinkInterface> &interface, ConfigurationParser &cfg);

JSBSim::FGFDMExec *_fdmexec; // FDMExec pointer
ConfigurationParser &_cfg;
Expand Down
6 changes: 3 additions & 3 deletions include/sensor_imu_plugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,9 +97,9 @@ class SensorImuPlugin : public SensorPlugin {
std::normal_distribution<double> _standard_normal_distribution;

/** Accelerations are affected by JSBSim airframe configuration <location name="EYEPOINT">
* ensure you have set the eyepoint location as to where you expect accelerometer measurements
* or more appropriately, use the px4_default_imu_sensor.xml configuration.
*/
* ensure you have set the eyepoint location as to where you expect accelerometer measurements
* or more appropriately, use the px4_default_imu_sensor.xml configuration.
*/
std::string _jsb_acc_x = "accelerations/a-pilot-x-ft_sec2";
std::string _jsb_acc_y = "accelerations/a-pilot-y-ft_sec2";
std::string _jsb_acc_z = "accelerations/a-pilot-z-ft_sec2";
Expand Down
61 changes: 37 additions & 24 deletions src/configuration_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,28 +54,39 @@ bool ConfigurationParser::ParseEnvironmentVariables() {
}

ArgResult ConfigurationParser::ParseArgV(int argc, char* const argv[]) {
static const struct option options[] = {
{"scene", required_argument, nullptr, 's'},
};
static const struct option options[] = {
{"scene", required_argument, nullptr, 's'},
{"device", required_argument, nullptr, 'd'},
{"baudrate", required_argument, nullptr, 'b'},
};

int c;
while ((c = getopt_long(argc, argv, "s:h", options, nullptr)) >= 0) {
switch (c) {
case 'h': {
return ArgResult::Help;
break;
}
case 's': {
_init_script_path = std::string(optarg);
break;
}
case '?':
default: {
std::cout << "Unknown Options" << std::endl;
return ArgResult::Error;
}
}
int c;
while ((c = getopt_long(argc, argv, "s:d:b:h", options, nullptr)) >= 0) {
switch (c) {
case 'h': {
return ArgResult::Help;
break;
}
case 's': {
_init_script_path = std::string(optarg);
break;
}
case 'd': {
_device = std::string(optarg);
_serial_enabled = true;
break;
}
case 'b': {
_baudrate = std::stoi(std::string(optarg));
break;
}
case '?':
default: {
std::cout << "Unknown Options" << std::endl;
return ArgResult::Error;
}
}
}

return ArgResult::Success;
}
Expand All @@ -99,9 +110,11 @@ bool ConfigurationParser::ParseConfigFile(const std::string& path) {
return true;
}

void ConfigurationParser::PrintHelpMessage(char *argv[]) {
void ConfigurationParser::PrintHelpMessage(char* argv[]) {
std::cout << argv[0] << " aircraft [options]\n\n"
<< " aircraft Aircraft config file name e.g. rascal"
<< " -h | --help Print available options\n"
<< " -s | --scene Location / scene where the vehicle should be spawned in e.g. LSZH\n";
<< " aircraft Aircraft config file name e.g. rascal"
<< " -h | --help Print available options\n"
<< " -s | --scene Location / scene where the vehicle should be spawned in e.g. LSZH\n"
<< " -d | --device Device path for FMU for HITL simulation e.g. /dev/ttyACM0\n"
<< " -b | --baudrate Device baudrate for FMU for HITL simulation e.g. 921600\n";
}
36 changes: 29 additions & 7 deletions src/jsbsim_bridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ JSBSimBridge::JSBSimBridge(JSBSim::FGFDMExec *fdmexec, ConfigurationParser &cfg)

// Configure Mavlink HIL interface
_mavlink_interface = std::make_unique<MavlinkInterface>();
SetMavlinkInterfaceConfigs(_mavlink_interface, config);
SetMavlinkInterfaceConfigs(_mavlink_interface, _cfg);

_mavlink_interface->Load();

Expand Down Expand Up @@ -131,8 +131,8 @@ bool JSBSimBridge::SetFdmConfigs(ConfigurationParser &cfg) {
std::string jsb_script;
if (config && CheckConfigElement(*config, "jsb_script")) {
std::size_t found = aircraft_path.rfind(aircraft_model);
if (found==std::string::npos) {
std::cout << "JSBSIM SCRIPT LOADING DOES NOT SUPPORT: " << aircraft_path << " <> " << aircraft_model << std::endl;
if (found == std::string::npos) {
std::cout << "JSBSIM SCRIPT LOADING DOES NOT SUPPORT: " << aircraft_path << " <> " << aircraft_model << std::endl;
return false;
} else {
_fdmexec->SetAircraftPath(SGPath("models/"));
Expand All @@ -149,18 +149,35 @@ bool JSBSimBridge::SetFdmConfigs(ConfigurationParser &cfg) {
}
}

bool JSBSimBridge::SetMavlinkInterfaceConfigs(std::unique_ptr<MavlinkInterface> &interface, TiXmlHandle &config) {
bool JSBSimBridge::SetMavlinkInterfaceConfigs(std::unique_ptr<MavlinkInterface> &interface, ConfigurationParser &cfg) {
TiXmlHandle config = *cfg.XmlHandle();
TiXmlElement *mavlink_configs = config.FirstChild("mavlink_interface").Element();

if (!mavlink_configs) return true; // Nothing to set

int tcp_port = kDefaultSITLTcpPort;
GetConfigElement<int>(config, "mavlink_interface", "tcp_port", tcp_port);
int udp_port = kDefaultGCSPort;
GetConfigElement<int>(config, "mavlink_interface", "udp_port", udp_port);
bool enable_lockstep = true;
GetConfigElement(config, "mavlink_interface", "enable_lockstep", enable_lockstep);

interface->SetMavlinkTcpPort(tcp_port);
interface->SetUseTcp(true);
if (cfg.getSerialEnabled()) {
// Configure for HITL when serial is enabled
interface->SetSerialEnabled(cfg.getSerialEnabled());
interface->SetDevice(cfg.getDevice());
interface->SetBaudrate(cfg.getBaudrate());
interface->SetHILStateLevel(true);
interface->SetHILMode(true);
interface->SetGcsAddr("INADDR_ANY");
interface->SetGcsUdpPort(udp_port);

} else {
// Configure for SITL as default
interface->SetMavlinkTcpPort(tcp_port);
interface->SetUseTcp(true);
}

interface->SetEnableLockstep(enable_lockstep);

return true;
Expand Down Expand Up @@ -199,7 +216,12 @@ void JSBSimBridge::Run() {
}

// Receive and handle actuator controls
_mavlink_interface->pollForMAVLinkMessages();
bool hil_mode_ = true;
if (hil_mode_) {
_mavlink_interface->pollFromGcsAndSdk();
} else {
_mavlink_interface->pollForMAVLinkMessages();
}
Eigen::VectorXd actuator_controls = _mavlink_interface->GetActuatorControls();

if (actuator_controls.size() >= 16) {
Expand Down
7 changes: 3 additions & 4 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,6 @@
#include "jsbsim_bridge.h"

int main(int argc, char *argv[]) {

// Parse Configurations
ConfigurationParser config;
if (argc > 1) {
Expand All @@ -51,15 +50,15 @@ int main(int argc, char *argv[]) {
config.ParseConfigFile(path);
}
switch (config.ParseArgV(argc, argv)) {
case ArgResult::Success : {
case ArgResult::Success: {
break;
}
case ArgResult::Help : {
case ArgResult::Help: {
ConfigurationParser::PrintHelpMessage(argv);
return 0;
}
default:
case ArgResult::Error : {
case ArgResult::Error: {
ConfigurationParser::PrintHelpMessage(argv);
return 1;
}
Expand Down
68 changes: 34 additions & 34 deletions src/sensor_mag_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,52 +78,52 @@ SensorData::Magnetometer SensorMagPlugin::getData() {
}

Eigen::Vector3d SensorMagPlugin::getMagFromJSBSim() {
if(_jsb_mag_x == "none" && _jsb_mag_y == "none" && _jsb_mag_z == "none") {
double lat_deg, lon_deg, roll_rad, pitch_rad, heading_rad;
if (_jsb_mag_x == "none" && _jsb_mag_y == "none" && _jsb_mag_z == "none") {
double lat_deg, lon_deg, roll_rad, pitch_rad, heading_rad;

lat_deg = _sim_ptr->GetPropertyValue(_jsb_mag_lat);
lon_deg = _sim_ptr->GetPropertyValue(_jsb_mag_lon);
roll_rad = _sim_ptr->GetPropertyValue(_jsb_mag_roll);
pitch_rad = _sim_ptr->GetPropertyValue(_jsb_mag_pitch);
heading_rad = wrap_pi(_sim_ptr->GetPropertyValue(_jsb_mag_hdg));
lat_deg = _sim_ptr->GetPropertyValue(_jsb_mag_lat);
lon_deg = _sim_ptr->GetPropertyValue(_jsb_mag_lon);
roll_rad = _sim_ptr->GetPropertyValue(_jsb_mag_roll);
pitch_rad = _sim_ptr->GetPropertyValue(_jsb_mag_pitch);
heading_rad = wrap_pi(_sim_ptr->GetPropertyValue(_jsb_mag_hdg));

// Magnetic strength (10^5xnanoTesla)
float strength_ga = 0.01f * get_mag_strength(lat_deg, lon_deg);
// Magnetic strength (10^5xnanoTesla)
float strength_ga = 0.01f * get_mag_strength(lat_deg, lon_deg);

// Magnetic declination and inclination (radians)
float declination_rad = get_mag_declination(lat_deg, lon_deg) * 3.14159265f / 180;
float inclination_rad = get_mag_inclination(lat_deg, lon_deg) * 3.14159265f / 180;
// Magnetic declination and inclination (radians)
float declination_rad = get_mag_declination(lat_deg, lon_deg) * 3.14159265f / 180;
float inclination_rad = get_mag_inclination(lat_deg, lon_deg) * 3.14159265f / 180;

// Magnetic filed components are calculated by http://geomag.nrcan.gc.ca/mag_fld/comp-en.php
float H = strength_ga * cosf(inclination_rad);
float Z = H * tanf(inclination_rad);
float X = H * cosf(declination_rad);
float Y = H * sinf(declination_rad);
// Magnetic filed components are calculated by http://geomag.nrcan.gc.ca/mag_fld/comp-en.php
float H = strength_ga * cosf(inclination_rad);
float Z = H * tanf(inclination_rad);
float X = H * cosf(declination_rad);
float Y = H * sinf(declination_rad);

_mag_g[0] = X;
_mag_g[1] = Y;
_mag_g[2] = Z;
_mag_g[0] = X;
_mag_g[1] = Y;
_mag_g[2] = Z;

Eigen::AngleAxisd rollAngle(roll_rad, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd yawAngle(heading_rad, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd pitchAngle(pitch_rad, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd rollAngle(roll_rad, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd yawAngle(heading_rad, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd pitchAngle(pitch_rad, Eigen::Vector3d::UnitY());

Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;

Eigen::Matrix3d rotationMatrix = q.inverse().matrix();
Eigen::Matrix3d rotationMatrix = q.inverse().matrix();

Eigen::Vector3d mag1 = rotationMatrix * _mag_g;
Eigen::Vector3d mag1 = rotationMatrix * _mag_g;

return mag1;
return mag1;

} else {
// Enable reading magnetic values directly from JSBSim when supplied with system file
// NOTE: Current JSBSim magnetometer startup sequence is not stable for PX4 in V1.1.0
// See https://github.com/JSBSim-Team/jsbsim/issues/332
_mag_g[0] = _sim_ptr->GetPropertyValue(_jsb_mag_x) * 1e-5;
_mag_g[1] = _sim_ptr->GetPropertyValue(_jsb_mag_y) * 1e-5;
_mag_g[2] = _sim_ptr->GetPropertyValue(_jsb_mag_z) * 1e-5;
return _mag_g;
// Enable reading magnetic values directly from JSBSim when supplied with system file
// NOTE: Current JSBSim magnetometer startup sequence is not stable for PX4 in V1.1.0
// See https://github.com/JSBSim-Team/jsbsim/issues/332
_mag_g[0] = _sim_ptr->GetPropertyValue(_jsb_mag_x) * 1e-5;
_mag_g[1] = _sim_ptr->GetPropertyValue(_jsb_mag_y) * 1e-5;
_mag_g[2] = _sim_ptr->GetPropertyValue(_jsb_mag_z) * 1e-5;
return _mag_g;
}
}

Expand Down

0 comments on commit d12eef0

Please sign in to comment.