diff --git a/configs/quadrotor_x.xml b/configs/quadrotor_x.xml index 7be5ca2..492c19a 100644 --- a/configs/quadrotor_x.xml +++ b/configs/quadrotor_x.xml @@ -4,6 +4,12 @@ + px4/acc-x + px4/acc-y + px4/acc-z + px4/gyro-x + px4/gyro-y + px4/gyro-z px4/gps-fix-type diff --git a/include/sensor_imu_plugin.h b/include/sensor_imu_plugin.h index 03a638a..8cdd16a 100644 --- a/include/sensor_imu_plugin.h +++ b/include/sensor_imu_plugin.h @@ -95,4 +95,17 @@ class SensorImuPlugin : public SensorPlugin { Eigen::Vector3d _accelerometer_turn_on_bias{Eigen::Vector3d::Zero()}; std::normal_distribution _standard_normal_distribution; + + /** Accelerations are affected by JSBSim airframe 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"; + + // 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"; }; diff --git a/models/quadrotor_x/quadrotor_x.xml b/models/quadrotor_x/quadrotor_x.xml index a491306..f2cf15a 100644 --- a/models/quadrotor_x/quadrotor_x.xml +++ b/models/quadrotor_x/quadrotor_x.xml @@ -452,5 +452,6 @@ + diff --git a/src/sensor_imu_plugin.cpp b/src/sensor_imu_plugin.cpp index 4f54f73..f8be80f 100644 --- a/src/sensor_imu_plugin.cpp +++ b/src/sensor_imu_plugin.cpp @@ -63,6 +63,12 @@ void SensorImuPlugin::setSensorConfigs(const TiXmlElement& configs) { GetConfigElement(configs, "accelerometer_random_walk", accelerometer_random_walk); GetConfigElement(configs, "accelerometer_bias_correlation_time", accelerometer_bias_correlation_time); GetConfigElement(configs, "accelerometer_turn_on_bias_sigma", accelerometer_turn_on_bias_sigma); + GetConfigElement(configs, "jsb_acc_x", _jsb_acc_x); + GetConfigElement(configs, "jsb_acc_y", _jsb_acc_y); + GetConfigElement(configs, "jsb_acc_z", _jsb_acc_z); + GetConfigElement(configs, "jsb_gyro_x", _jsb_gyro_x); + GetConfigElement(configs, "jsb_gyro_y", _jsb_gyro_y); + GetConfigElement(configs, "jsb_gyro_z", _jsb_gyro_z); } SensorData::Imu SensorImuPlugin::getData() { @@ -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); } diff --git a/systems/px4_default_imu_sensor.xml b/systems/px4_default_imu_sensor.xml new file mode 100644 index 0000000..55ed97a --- /dev/null +++ b/systems/px4_default_imu_sensor.xml @@ -0,0 +1,161 @@ + + + + + + px4/acc_x/on + px4/acc_y/on + px4/acc_z/on + + px4/gyro_x/on + px4/gyro_y/on + px4/gyro_z/on + + + + + 0 + 0 + 0.345 + + + X + px4/acc-x + + + + + + + 0 + 0 + 0.345 + + + Y + px4/acc-y + + + + + + + 0 + 0 + 0.345 + + + Z + px4/acc-z + + + + + + velocities/p-rad_sec + 0 + + + + + + + velocities/q-rad_sec + 0 + + + + + + + velocities/r-rad_sec + 0 + + + + + +