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

Enable user defined IMU Parameters from JSBSim #24

Merged
merged 8 commits into from
Oct 16, 2020
+193 −6
Merged

Enable user defined IMU Parameters from JSBSim #24

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
7a86536
Add px4_default_imu_sensor.xml
mvacanti Oct 14, 2020
8085246
Include mapping to JSBSim IMU parameters.
mvacanti Oct 14, 2020
429e06f
Include system file mapping for px4_default_imu_sensor
mvacanti Oct 14, 2020
dff7d9b
Enable XML mapping of JSBSim parameters
mvacanti Oct 14, 2020
729dfea
Move default values of JSBSim properties
mvacanti Oct 14, 2020
84a1d64
Merge branch 'master' of https://github.com/Auterion/px4-jsbsim-bridg…
mvacanti Oct 14, 2020
53f41fd
Minor comment change
mvacanti Oct 14, 2020
02df55c
Naming convention update
mvacanti Oct 16, 2020
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
6 changes: 6 additions & 0 deletions configs/quadrotor_x.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,12 @@
</mavlink_interface>
<sensors>
<imu>
<jsb_acc_x>px4/acc-x</jsb_acc_x>
<jsb_acc_y>px4/acc-y</jsb_acc_y>
<jsb_acc_z>px4/acc-z</jsb_acc_z>
<jsb_gyro_x>px4/gyro-x</jsb_gyro_x>
<jsb_gyro_y>px4/gyro-y</jsb_gyro_y>
<jsb_gyro_z>px4/gyro-z</jsb_gyro_z>
</imu>
<gps>
<jsb_gps_fix_type>px4/gps-fix-type</jsb_gps_fix_type>
Expand Down
13 changes: 13 additions & 0 deletions include/sensor_imu_plugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,4 +95,17 @@ class SensorImuPlugin : public SensorPlugin {
Eigen::Vector3d _accelerometer_turn_on_bias{Eigen::Vector3d::Zero()};

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.
*/
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";

// PX4 requires axis angular acceleration vs. body frame acceleration.
std::string _jsb_gyro_x = "velocities/p-rad_sec";
std::string _jsb_gyro_y = "velocities/q-rad_sec";
std::string _jsb_gyro_z = "velocities/r-rad_sec";
};
1 change: 1 addition & 0 deletions models/quadrotor_x/quadrotor_x.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -452,5 +452,6 @@
</function>
</axis>
</aerodynamics>
<system file="px4_default_imu_sensor"/>
<system file="px4_default_gps_sensor"/>
</fdm_config>
18 changes: 12 additions & 6 deletions src/sensor_imu_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,12 @@ void SensorImuPlugin::setSensorConfigs(const TiXmlElement& configs) {
GetConfigElement<double>(configs, "accelerometer_random_walk", accelerometer_random_walk);
GetConfigElement<double>(configs, "accelerometer_bias_correlation_time", accelerometer_bias_correlation_time);
GetConfigElement<double>(configs, "accelerometer_turn_on_bias_sigma", accelerometer_turn_on_bias_sigma);
GetConfigElement<std::string>(configs, "jsb_acc_x", _jsb_acc_x);
GetConfigElement<std::string>(configs, "jsb_acc_y", _jsb_acc_y);
GetConfigElement<std::string>(configs, "jsb_acc_z", _jsb_acc_z);
GetConfigElement<std::string>(configs, "jsb_gyro_x", _jsb_gyro_x);
GetConfigElement<std::string>(configs, "jsb_gyro_y", _jsb_gyro_y);
GetConfigElement<std::string>(configs, "jsb_gyro_z", _jsb_gyro_z);
}

SensorData::Imu SensorImuPlugin::getData() {
Expand All @@ -83,17 +89,17 @@ SensorData::Imu SensorImuPlugin::getData() {
}

Eigen::Vector3d SensorImuPlugin::getAccelFromJSBSim() {
double x = _sim_ptr->GetPropertyValue("accelerations/a-pilot-x-ft_sec2");
double y = _sim_ptr->GetPropertyValue("accelerations/a-pilot-y-ft_sec2");
double z = _sim_ptr->GetPropertyValue("accelerations/a-pilot-z-ft_sec2");
double x = _sim_ptr->GetPropertyValue(_jsb_acc_x);
double y = _sim_ptr->GetPropertyValue(_jsb_acc_y);
double z = _sim_ptr->GetPropertyValue(_jsb_acc_z);

return Eigen::Vector3d(ftToM(x), ftToM(y), ftToM(z));
}

Eigen::Vector3d SensorImuPlugin::getGyroFromJSBSim() {
double x = _sim_ptr->GetPropertyValue("velocities/p-rad_sec");
double y = _sim_ptr->GetPropertyValue("velocities/q-rad_sec");
double z = _sim_ptr->GetPropertyValue("velocities/r-rad_sec");
double x = _sim_ptr->GetPropertyValue(_jsb_gyro_x);
double y = _sim_ptr->GetPropertyValue(_jsb_gyro_y);
double z = _sim_ptr->GetPropertyValue(_jsb_gyro_z);

return Eigen::Vector3d(x, y, z);
}
Expand Down
161 changes: 161 additions & 0 deletions systems/px4_default_imu_sensor.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,161 @@
<?xml version="1.0"?>
<system name="PX4 IMU Sensor">
<!-- =================================================================
This is a generic JSBSim system file to simulate the IMU behavior for PX4.
================================================================= -->

<!-- Enable Individual Component Failure -->
<property value="1">px4/acc_x/on</property>
<property value="1">px4/acc_y/on</property>
<property value="1">px4/acc_z/on</property>

<property value="1">px4/gyro_x/on</property>
<property value="1">px4/gyro_y/on</property>
<property value="1">px4/gyro_z/on</property>

<channel name="Accelerometer-X" execute="px4/acc_x/on">
<accelerometer name="Default-ACC-X">
<location unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0.345 </z>
</location>
Comment on lines +18 to +22
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a way to avoid duplicating this for the other axes?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe this is a constraint of the JSBSim accelerometer function. There are a few functions that allow XML variables to be passed so that you could accomplish something like this where the property is declared once:

                <x> <property> px4/imu/loc_x </property>  </x>
                <y> <property> px4/imu/loc_y </property>  </y>
                <z> <property> px4/imu/loc_z </property>  </z>

I dug into the backend of JSBSim (and tried a few combination of the above) and unfortunately the only input it accepts are numeric.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok, let's keep it as is then.

<!--
<orientation unit="DEG">
<pitch> 0 </pitch>
<roll> 0 </roll>
<yaw> 0 </yaw>
</orientation>
<lag> 0 </lag>
<noise variation="PERCENT|ABSOLUTE"> number </noise>
<quantization name="name">
<bits> number </bits>
<min> number </min>
<max> number </max>
</quantization>
<drift_rate> number </drift_rate>
<gain> number </gain>
<bias> number </bias>
-->
<axis> X </axis>
<output> px4/acc-x </output>
</accelerometer>
</channel>

<channel name="Accelerometer-Y" execute="px4/acc_y/on">
<accelerometer name="Default-ACC-Y">
<location unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0.345 </z>
</location>
<!--
<orientation unit="DEG">
<pitch> 0 </pitch>
<roll> 0 </roll>
<yaw> 0 </yaw>
</orientation>
<lag> 0 </lag>
<noise variation="PERCENT|ABSOLUTE"> number </noise>
<quantization name="name">
<bits> number </bits>
<min> number </min>
<max> number </max>
</quantization>
<drift_rate> number </drift_rate>
<gain> number </gain>
<bias> number </bias>
-->
<axis> Y </axis>
<output> px4/acc-y </output>
</accelerometer>
</channel>

<channel name="Accelerometer-Z" execute="px4/acc_z/on">
<accelerometer name="Default-ACC-Z">
<location unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0.345 </z>
</location>
<!--
<orientation unit="DEG">
<pitch> 0 </pitch>
<roll> 0 </roll>
<yaw> 0 </yaw>
</orientation>
<lag> 0 </lag>
<noise variation="PERCENT|ABSOLUTE"> number </noise>
<quantization name="name">
<bits> number </bits>
<min> number </min>
<max> number </max>
</quantization>
<drift_rate> number </drift_rate>
<gain> number </gain>
<bias> number </bias>
-->
<axis> Z </axis>
<output> px4/acc-z </output>
</accelerometer>
</channel>

<channel name="Gyro-X" execute="px4/gyro_x/on">
<sensor name="px4/gyro-x">
<input>velocities/p-rad_sec</input>
<lag> 0 </lag>
<!--Adjust as required
<noise variation="PERCENT|ABSOLUTE"> number </noise>
<quantization name="name">
<bits> number </bits>
<min> number </min>
<max> number </max>
</quantization>
<drift_rate> number </drift_rate>
<gain> number </gain>
<bias> number </bias>
<delay [type="time|frames"]> number < /delay>
-->
</sensor>
</channel>

<channel name="Gyro-Y" execute="px4/gyro_y/on">
<sensor name="px4/gyro-y">
<input>velocities/q-rad_sec</input>
<lag> 0 </lag>
<!--Adjust as required
<noise variation="PERCENT|ABSOLUTE"> number </noise>
<quantization name="name">
<bits> number </bits>
<min> number </min>
<max> number </max>
</quantization>
<drift_rate> number </drift_rate>
<gain> number </gain>
<bias> number </bias>
<delay [type="time|frames"]> number < /delay>
-->
</sensor>
</channel>

<channel name="Gyro-Z" execute="px4/gyro_z/on">
<sensor name="px4/gyro-z">
<input>velocities/r-rad_sec</input>
<lag> 0 </lag>
<!--Adjust as required
<noise variation="PERCENT|ABSOLUTE"> number </noise>
<quantization name="name">
<bits> number </bits>
<min> number </min>
<max> number </max>
</quantization>
<drift_rate> number </drift_rate>
<gain> number </gain>
<bias> number </bias>
<delay [type="time|frames"]> number < /delay>
-->
</sensor>
</channel>

</system>