dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

CollisionPrevention add more distance_sensor orientations

This commit is contained in:
Daniel Agar 2019-09-10 17:38:17 -04:00
parent 5126e6ea61
commit 0fe271b7f5
3 changed files with 68 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
msg/distance_sensor.msg
View File

@ -16,15 +16,27 @@ uint8 MAV_DISTANCE_SENSOR_RADAR = 3
uint8 id # Onboard ID of the sensor
uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270
uint8 ROTATION_UPWARD_FACING = 24 # MAV_SENSOR_ROTATION_PITCH_90
uint8 ROTATION_BACKWARD_FACING = 12 # MAV_SENSOR_ROTATION_PITCH_180
uint8 ROTATION_FORWARD_FACING = 0 # MAV_SENSOR_ROTATION_NONE
uint8 ROTATION_LEFT_FACING = 6 # MAV_SENSOR_ROTATION_YAW_270
uint8 ROTATION_RIGHT_FACING = 2 # MAV_SENSOR_ROTATION_YAW_90
uint8 ROTATION_CUSTOM =100 # MAV_SENSOR_ROTATION_CUSTOM
float32 h_fov # Sensor horizontal field of view (rad)
float32 v_fov # Sensor vertical field of view (rad)
float32[4] q # Quaterion sensor orientation with respect to the vehicle body frame to specify the orientation ROTATION_CUSTOM
uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
uint8 ROTATION_YAW_0 = 0 # MAV_SENSOR_ROTATION_NONE
uint8 ROTATION_YAW_45 = 1 # MAV_SENSOR_ROTATION_YAW_45
uint8 ROTATION_YAW_90 = 2 # MAV_SENSOR_ROTATION_YAW_90
uint8 ROTATION_YAW_135 = 3 # MAV_SENSOR_ROTATION_YAW_135
uint8 ROTATION_YAW_180 = 4 # MAV_SENSOR_ROTATION_YAW_180
uint8 ROTATION_YAW_225 = 5 # MAV_SENSOR_ROTATION_YAW_225
uint8 ROTATION_YAW_270 = 6 # MAV_SENSOR_ROTATION_YAW_270
uint8 ROTATION_YAW_315 = 7 # MAV_SENSOR_ROTATION_YAW_315
uint8 ROTATION_FORWARD_FACING = 0 # MAV_SENSOR_ROTATION_NONE
uint8 ROTATION_RIGHT_FACING = 2 # MAV_SENSOR_ROTATION_YAW_90
uint8 ROTATION_BACKWARD_FACING = 4 # MAV_SENSOR_ROTATION_YAW_180
uint8 ROTATION_LEFT_FACING = 6 # MAV_SENSOR_ROTATION_YAW_270
uint8 ROTATION_UPWARD_FACING = 24 # MAV_SENSOR_ROTATION_PITCH_90
uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270
uint8 ROTATION_CUSTOM = 100 # MAV_SENSOR_ROTATION_CUSTOM

46
src/lib/CollisionPrevention/CollisionPrevention.cpp
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)

26
src/lib/CollisionPrevention/CollisionPrevention.hpp
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