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

CollisionPrevention add more distance_sensor orientations

Browse Source
This commit is contained in:
Daniel Agar
2019-10-01 21:05:37 -04:00
parent 5126e6ea61
commit 0fe271b7f5
3 changed files with 68 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+46
View File
@@ -323,6 +323,52 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
}
}
float
CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
{
float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f;
switch (distance_sensor.orientation) {
case distance_sensor_s::ROTATION_YAW_0:
offset = 0.0f;
break;
case distance_sensor_s::ROTATION_YAW_45:
offset = M_PI_F / 4.0f;
break;
case distance_sensor_s::ROTATION_YAW_90:
offset = M_PI_F / 2.0f;
break;
case distance_sensor_s::ROTATION_YAW_135:
offset = 3.0f * M_PI_F / 4.0f;
break;
case distance_sensor_s::ROTATION_YAW_180:
offset = M_PI_F;
break;
case distance_sensor_s::ROTATION_YAW_225:
offset = -3.0f * M_PI_F / 4.0f;
break;
case distance_sensor_s::ROTATION_YAW_270:
offset = -M_PI_F / 2.0f;
break;
case distance_sensor_s::ROTATION_YAW_315:
offset = -M_PI_F / 4.0f;
break;
case distance_sensor_s::ROTATION_CUSTOM:
offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
break;
}
return offset;
}
void
CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
const Vector2f &curr_vel)
src/lib/CollisionPrevention/CollisionPrevention.hpp
+1 -25
View File
@@ -151,31 +151,7 @@ private:
* @param distance_sensor, distance sensor message
* @param angle_offset, sensor body frame offset
*/
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;
}
float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const;
/**
* Computes collision free setpoints