Skip to content

Commit

Permalink
Add device /baudrate for configuration options
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
  • Loading branch information
@Jaeyoung-Lim
Jaeyoung-Lim committed Oct 22, 2020
1 parent aa8997c commit 7a7aff1
Show file tree
Hide file tree
Showing 8 changed files with 124 additions and 97 deletions.
17 changes: 11 additions & 6 deletions include/configuration_parser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,27 +42,27 @@

#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();
bool ParseConfigFile(const std::string& path);
ArgResult ParseArgV(int argc, char* const argv[]);
inline bool isHeadless() { return _headless; }
inline std::shared_ptr<TiXmlHandle> XmlHandle() { return _config; }
inline std::string getDevice() { return _device; }
inline std::string getInitScriptPath() { return _init_script_path; }
inline std::string getModelName() { return _model_name; }
static void PrintHelpMessage(char *argv[]);
inline bool getSerialEnabled() { return _serial_enabled; }
inline int getBaudrate() { return _baudrate; }
static void PrintHelpMessage(char* argv[]);

private:
TiXmlDocument _doc;
Expand All @@ -71,4 +71,9 @@ class ConfigurationParser {
bool _headless{false};
std::string _init_script_path;
std::string _model_name;

// HITL Configs
bool _serial_enabled{false};
std::string _device;
int _baudrate{921600};
};
2 changes: 1 addition & 1 deletion include/jsbsim_bridge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,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 @@ -50,28 +50,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 @@ -95,9 +106,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";
}
22 changes: 16 additions & 6 deletions src/jsbsim_bridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,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 @@ -132,8 +132,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 @@ -150,7 +150,8 @@ 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
Expand All @@ -160,8 +161,17 @@ bool JSBSimBridge::SetMavlinkInterfaceConfigs(std::unique_ptr<MavlinkInterface>
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());
} else {
// Configure for SITL as default
interface->SetMavlinkTcpPort(tcp_port);
interface->SetUseTcp(true);
}

interface->SetEnableLockstep(enable_lockstep);

return true;
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
38 changes: 19 additions & 19 deletions src/sensor_gps_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,17 +46,17 @@ SensorGpsPlugin::SensorGpsPlugin(JSBSim::FGFDMExec* jsbsim) : SensorPlugin(jsbsi
SensorGpsPlugin::~SensorGpsPlugin() {}

void SensorGpsPlugin::setSensorConfigs(const TiXmlElement& configs) {
GetConfigElement<std::string>(configs, "jsb_gps_fix_type", jsb_gps_fix_type);
GetConfigElement<std::string>(configs, "jsb_gps_lat", jsb_gps_lat);
GetConfigElement<std::string>(configs, "jsb_gps_lon", jsb_gps_lon);
GetConfigElement<std::string>(configs, "jsb_gps_alt", jsb_gps_alt);
GetConfigElement<std::string>(configs, "jsb_gps_eph", jsb_gps_eph);
GetConfigElement<std::string>(configs, "jsb_gps_epv", jsb_gps_epv);
GetConfigElement<std::string>(configs, "jsb_gps_v_north", jsb_gps_v_north);
GetConfigElement<std::string>(configs, "jsb_gps_v_east", jsb_gps_v_east);
GetConfigElement<std::string>(configs, "jsb_gps_v_down", jsb_gps_v_down);
GetConfigElement<std::string>(configs, "jsb_gps_velocity", jsb_gps_velocity);
GetConfigElement<std::string>(configs, "jsb_gps_satellites", jsb_gps_satellites);
GetConfigElement<std::string>(configs, "jsb_gps_fix_type", jsb_gps_fix_type);
GetConfigElement<std::string>(configs, "jsb_gps_lat", jsb_gps_lat);
GetConfigElement<std::string>(configs, "jsb_gps_lon", jsb_gps_lon);
GetConfigElement<std::string>(configs, "jsb_gps_alt", jsb_gps_alt);
GetConfigElement<std::string>(configs, "jsb_gps_eph", jsb_gps_eph);
GetConfigElement<std::string>(configs, "jsb_gps_epv", jsb_gps_epv);
GetConfigElement<std::string>(configs, "jsb_gps_v_north", jsb_gps_v_north);
GetConfigElement<std::string>(configs, "jsb_gps_v_east", jsb_gps_v_east);
GetConfigElement<std::string>(configs, "jsb_gps_v_down", jsb_gps_v_down);
GetConfigElement<std::string>(configs, "jsb_gps_velocity", jsb_gps_velocity);
GetConfigElement<std::string>(configs, "jsb_gps_satellites", jsb_gps_satellites);
}

SensorData::Gps SensorGpsPlugin::getData() {
Expand All @@ -75,37 +75,37 @@ SensorData::Gps SensorGpsPlugin::getGpsFromJSBSim() {
SensorData::Gps ret;
ret.time_utc_usec = _sim_ptr->GetSimTime() * 1e6;

if(jsb_gps_fix_type == "none") {
if (jsb_gps_fix_type == "none") {
ret.fix_type = 3;
} else {
ret.fix_type = _sim_ptr->GetPropertyValue(jsb_gps_fix_type);
ret.fix_type = _sim_ptr->GetPropertyValue(jsb_gps_fix_type);
}

ret.latitude_deg = _sim_ptr->GetPropertyValue(jsb_gps_lat) * 1e7;
ret.longitude_deg = _sim_ptr->GetPropertyValue(jsb_gps_lon) * 1e7;
ret.altitude = _sim_ptr->GetPropertyValue(jsb_gps_alt) * 1e3;

if(jsb_gps_eph == "none") {
if (jsb_gps_eph == "none") {
ret.eph = 1 * 100;
} else {
ret.eph = _sim_ptr->GetPropertyValue(jsb_gps_eph)*100;
ret.eph = _sim_ptr->GetPropertyValue(jsb_gps_eph) * 100;
}

if(jsb_gps_epv == "none") {
if (jsb_gps_epv == "none") {
ret.epv = 2 * 100;
} else {
ret.epv = _sim_ptr->GetPropertyValue(jsb_gps_epv)*100;
ret.epv = _sim_ptr->GetPropertyValue(jsb_gps_epv) * 100;
}

ret.velocity_north = ftToM(_sim_ptr->GetPropertyValue(jsb_gps_v_north)) * 100;
ret.velocity_east = ftToM(_sim_ptr->GetPropertyValue(jsb_gps_v_east)) * 100;
ret.velocity_down = ftToM(_sim_ptr->GetPropertyValue(jsb_gps_v_down)) * 100;
ret.velocity = ftToM(_sim_ptr->GetPropertyValue(jsb_gps_velocity)) * 100;

if(jsb_gps_satellites == "none") {
if (jsb_gps_satellites == "none") {
ret.satellites_visible = 16;
} else {
ret.satellites_visible = _sim_ptr->GetPropertyValue(jsb_gps_satellites);
ret.satellites_visible = _sim_ptr->GetPropertyValue(jsb_gps_satellites);
}

ret.id = 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 7a7aff1

Please sign in to comment.