fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 07:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

WIP: add X3

Browse Source
This commit is contained in:
Daniel Agar
2022-08-14 15:15:24 -04:00
parent 61226a39bd
commit 04ac07f07a
16 changed files with 10971 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ignition_simulator/IgnitionSimulator.cpp
+18 -13
View File
@@ -44,21 +44,26 @@
void IgnitionSimulator::ImuCallback(const ignition::msgs::IMU &imu)
{
// FLU -> FRD
_px4_accel.update(hrt_absolute_time(),
imu.linear_acceleration().x(),
-imu.linear_acceleration().y(),
-imu.linear_acceleration().z());
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
_px4_gyro.update(hrt_absolute_time(),
imu.angular_velocity().x(),
-imu.angular_velocity().y(),
-imu.angular_velocity().z());
ignition::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
imu.linear_acceleration().x(),
imu.linear_acceleration().y(),
imu.linear_acceleration().z()));
ignition::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
imu.angular_velocity().x(),
imu.angular_velocity().y(),
imu.angular_velocity().z()));
_px4_accel.update(hrt_absolute_time(), accel_b.X(), accel_b.Y(), accel_b.Z());
_px4_gyro.update(hrt_absolute_time(), gyro_b.X(), gyro_b.Y(), gyro_b.Z());
}
void IgnitionSimulator::PoseInfoCallback(const ignition::msgs::Pose_V &pose)
{
for (int p = 0; p < pose.pose_size(); p++) {
std::string vehicle_name = "X4";
std::string vehicle_name = "X3";
if (pose.pose(p).name() == vehicle_name) {
const hrt_abstime time_now_us = hrt_absolute_time();
@@ -118,8 +123,8 @@ void IgnitionSimulator::PoseInfoCallback(const ignition::msgs::Pose_V &pose)
vehicle_local_position_s local_position_groundtruth{};
local_position_groundtruth.timestamp_sample = hrt_absolute_time();
const matrix::Vector3d position{pose_position.x(), pose_position.y(), pose_position.z()};
// position ENU -> NED
const matrix::Vector3d position{pose_position.y(), pose_position.x(), -pose_position.z()};
const matrix::Vector3d velocity{(position - _position_prev) / dt};
const matrix::Vector3d acceleration{(velocity - _velocity_prev) / dt};
@@ -175,11 +180,11 @@ void IgnitionSimulator::run()
ignition::transport::Node node;
// Subscribe to messages of other plugins.
node.Subscribe("/world/quadcopter/model/X4/link/base_link/sensor/imu_sensor/imu", &IgnitionSimulator::ImuCallback,
node.Subscribe("/world/quadcopter/model/X3/link/base_link/sensor/imu_sensor/imu", &IgnitionSimulator::ImuCallback,
this);
node.Subscribe("/world/quadcopter/pose/info", &IgnitionSimulator::PoseInfoCallback, this);
std::string topic = "/X4/command/motor_speed";
std::string topic = "/X3/command/motor_speed";
auto pub = node.Advertise<ignition::msgs::Actuators>(topic);
ignition::msgs::Actuators rotor_velocity_message;