fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 02:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

CollisionPrevention: refactor code to make it more readable

Browse Source
This commit is contained in:
Martina Rivizzigno
2019-07-15 10:58:00 +02:00
committed by Beat Küng
parent b6eea508bb
commit 2439dc09ae
2 changed files with 27 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+1 -20
View File
@@ -125,26 +125,7 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle)
if ((distance_sensor.current_distance > distance_sensor.min_distance) &&
(distance_sensor.current_distance < distance_sensor.max_distance)) {
// init offset for sensor orientation distance_sensor_s::ROTATION_FORWARD_FACING or with offset coming from the companion
float sensor_yaw_body_rad = obstacle.angle_offset > 0.f ? math::radians(obstacle.angle_offset) : 0.0f;
switch (distance_sensor.orientation) {
case distance_sensor_s::ROTATION_RIGHT_FACING:
sensor_yaw_body_rad = M_PI_F / 2.0f;
break;
case distance_sensor_s::ROTATION_LEFT_FACING:
sensor_yaw_body_rad = -M_PI_F / 2.0f;
break;
case distance_sensor_s::ROTATION_BACKWARD_FACING:
sensor_yaw_body_rad = M_PI_F;
break;
case distance_sensor_s::ROTATION_CUSTOM:
sensor_yaw_body_rad = Eulerf(Quatf(distance_sensor.q)).psi();
break;
}
float sensor_yaw_body_rad = sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset);
matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
// convert the sensor orientation from body to local frame in the range [0, 360]