Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 14:56:51 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

CollisionPrevention: refactor code to make it more readable

Browse Source
This commit is contained in:
Martina Rivizzigno
2019-06-05 18:43:15 +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]
src/lib/CollisionPrevention/CollisionPrevention.hpp
+26
View File
@@ -90,6 +90,32 @@ private:
void update();
inline float sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset)
{
float offset = angle_offset > 0.f ? math::radians(angle_offset) : 0.0f;
switch (distance_sensor.orientation) {
case distance_sensor_s::ROTATION_RIGHT_FACING:
offset = M_PI_F / 2.0f;
break;
case distance_sensor_s::ROTATION_LEFT_FACING:
offset = -M_PI_F / 2.0f;
break;
case distance_sensor_s::ROTATION_BACKWARD_FACING:
offset = M_PI_F;
break;
case distance_sensor_s::ROTATION_CUSTOM:
offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
break;
}
return offset;
}
void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos,
const matrix::Vector2f &curr_vel);