From 1ce5b73d7390102650487b09e148ddce77ac2192 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 3 Jul 2020 23:29:47 +0530 Subject: [PATCH] Use custom sensor orientation in Mavlink Distance sensor message --- .../firmwares/mavlink/MavLinkMultirotorApi.hpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index 5bb8b6e470..d13185d94b 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -116,15 +116,13 @@ class MavLinkMultirotorApi : public MultirotorApiBase const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); if (count_distance_sensors != 0) { const auto& distance_output = getDistanceSensorData(""); - float pitch, roll, yaw; - VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw); sendDistanceSensor(distance_output.min_distance / 100, //m -> cm distance_output.max_distance / 100, //m -> cm distance_output.distance, 0, //sensor type: //TODO: allow changing in settings? 77, //sensor id, //TODO: should this be something real? - pitch); //TODO: convert from radians to degrees? + distance_output.relative_pose.orientation); //TODO: convert from radians to degrees? } const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps); @@ -1379,7 +1377,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase last_sensor_message_ = hil_sensor; } - void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, float orientation) + void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, Quaternionr orientation) { if (!is_simulation_mode_) throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode"); @@ -1392,7 +1390,14 @@ class MavLinkMultirotorApi : public MultirotorApiBase distance_sensor.current_distance = static_cast(current_distance); distance_sensor.type = static_cast(sensor_type); distance_sensor.id = static_cast(sensor_id); - distance_sensor.orientation = static_cast(orientation); + + // Use custom orientation + distance_sensor.orientation = 100; // MAV_SENSOR_ROTATION_CUSTOM + distance_sensor.quaternion[0] = orientation.w(); + distance_sensor.quaternion[1] = orientation.x(); + distance_sensor.quaternion[2] = orientation.y(); + distance_sensor.quaternion[3] = orientation.z(); + //TODO: use covariance parameter? if (hil_node_ != nullptr) {