yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

map mavlink orientation enum to uorb distance_sensor orientation

Browse Source
This commit is contained in:
Martina Rivizzigno
2020-02-25 16:09:19 +01:00
committed by Beat Küng
parent 779ba0fda5
commit abe33af5b3
1 changed files with 30 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -1125,9 +1125,38 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl
dist.current_distance = dist_mavlink->current_distance / 100.0f;
dist.type = dist_mavlink->type;
dist.id = dist_mavlink->id;
dist.orientation = dist_mavlink->orientation;
dist.variance = dist_mavlink->covariance * 1e-4f; // cm^2 to m^2
dist.signal_quality = -1;
switch (dist_mavlink->orientation) {
case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_270:
dist.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
break;
case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_90:
dist.orientation = distance_sensor_s::ROTATION_UPWARD_FACING;
break;
case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_180:
dist.orientation = distance_sensor_s::ROTATION_BACKWARD_FACING;
break;
case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_NONE:
dist.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
break;
case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_270:
dist.orientation = distance_sensor_s::ROTATION_LEFT_FACING;
break;
case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_90:
dist.orientation = distance_sensor_s::ROTATION_RIGHT_FACING;
break;
default:
dist.orientation = distance_sensor_s::ROTATION_CUSTOM;
}
dist.h_fov = dist_mavlink->horizontal_fov;
dist.v_fov = dist_mavlink->vertical_fov;
dist.q[0] = dist_mavlink->quaternion[0];