Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add support for HIL #64

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add support for HIL #64

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions include/configuration_parser.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,9 @@ class ConfigurationParser {
std::string getInitScriptPath() { return _init_script_path; }
std::string getModelName() { return _model_name; }
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 @@ -72,4 +75,9 @@ 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};
};

4 changes: 3 additions & 1 deletion include/jsbsim_bridge.h
View file
Open in desktop
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 All @@ -84,3 +85,4 @@ class JSBSimBridge {
double _realtime_factor{1.0};
bool _result{true};
};

22 changes: 18 additions & 4 deletions src/configuration_parser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,10 +56,12 @@ bool ConfigurationParser::ParseEnvironmentVariables() {
ArgResult ConfigurationParser::ParseArgV(int argc, char* const argv[]) {
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) {
while ((c = getopt_long(argc, argv, "s:d:b:h", options, nullptr)) >= 0) {
switch (c) {
case 'h': {
return ArgResult::Help;
Expand All @@ -69,6 +71,15 @@ ArgResult ConfigurationParser::ParseArgV(int argc, char* const argv[]) {
_init_script_path = std::string(optarg);
break;
}
case 'd': {
_device = std::string(optarg);
_serial_enabled = true;
break;
}
case 'b': {
_baudrate = atoi(optarg);
break;
}
case '?':
default: {
std::cout << "Unknown Options" << std::endl;
Expand Down Expand Up @@ -101,7 +112,10 @@ bool ConfigurationParser::ParseConfigFile(const std::string& path) {

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";
}

37 changes: 29 additions & 8 deletions src/jsbsim_bridge.cpp
View file
Open in desktop
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 @@ -83,7 +83,7 @@ JSBSimBridge::JSBSimBridge(JSBSim::FGFDMExec *fdmexec, ConfigurationParser &cfg)

if (CheckConfigElement(config, "sensors", "airspeed")) {
_airspeed_sensor = std::make_unique<SensorAirspeedPlugin>(_fdmexec);
_airspeed_sensor->setSensorConfigs(GetXmlElement(config, "sensors", "airspeed"));
_mag_sensor->setSensorConfigs(GetXmlElement(config, "sensors", "airspeed"));
}

_actuators = std::make_unique<ActuatorPlugin>(_fdmexec);
Expand Down Expand Up @@ -149,18 +149,33 @@ 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;
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->SetGcsUdpPort(udp_port);

if (cfg.getSerialEnabled()) {
// Configure for HITL when serial is enabled
interface->SetSerialEnabled(cfg.getSerialEnabled());
interface->SetDevice(cfg.getDevice());
interface->SetBaudrate(cfg.getBaudrate());
interface->SetHILMode(true);
cout <<"hilmode is enabled"<<endl;
} else {
// Configure for SITL as default
interface->SetMavlinkTcpPort(tcp_port);
interface->SetUseTcp(true);
}

interface->SetMavlinkTcpPort(tcp_port);
interface->SetUseTcp(true);
interface->SetEnableLockstep(enable_lockstep);

return true;
Expand Down Expand Up @@ -199,7 +214,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 All @@ -212,7 +232,8 @@ void JSBSimBridge::Run() {

std::chrono::duration<double> elapsed_time = step_start_time - step_stop_time;
if (_realtime_factor > 0) {
double sleep = _dt / _realtime_factor - elapsed_time.count();
double sleep = _dt/_realtime_factor - elapsed_time.count();
if (sleep > 0) usleep(sleep * 1e6);
}
}