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

CollisionPrevention: use FlightTasks convention for private/public methods, 

add doxygen on header file
This commit is contained in:
Martina Rivizzigno 2019-06-25 14:58:08 +02:00 committed by Beat Küng
parent 8637a9255e
commit 02bdc2c46b
2 changed files with 59 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/lib/CollisionPrevention/CollisionPrevention.cpp
View File

@ -55,7 +55,7 @@ CollisionPrevention::~CollisionPrevention()
}
}
void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_setpoint,
void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint,
const Vector2f &adapted_setpoint)
{
collision_constraints_s constraints{}; /**< collision constraints message */
@ -77,7 +77,7 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se
}
}
void CollisionPrevention::publishObstacleDistance(obstacle_distance_s &obstacle)
void CollisionPrevention::_publishObstacleDistance(obstacle_distance_s &obstacle)
{
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
if (_obstacle_distance_pub != nullptr) {
@ -88,7 +88,7 @@ void CollisionPrevention::publishObstacleDistance(obstacle_distance_s &obstacle)
}
}
void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &obstacle)
void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle)
{
_sub_obstacle_distance.update();
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
@ -99,11 +99,9 @@ void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &ob
}
}
void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle)
void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
{
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
// _sub_distance_sensor[i].update();
// const distance_sensor_s &distance_sensor = _sub_distance_sensor[i].get();
distance_sensor_s distance_sensor;
_sub_distance_sensor[i].copy(&distance_sensor);
@ -136,7 +134,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)) {
float sensor_yaw_body_rad = sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset);
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]
@ -183,15 +181,15 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle)
}
}
publishObstacleDistance(obstacle);
_publishObstacleDistance(obstacle);
}
void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint,
void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
const Vector2f &curr_pos, const Vector2f &curr_vel)
{
obstacle_distance_s obstacle = {};
updateOffboardObstacleDistance(obstacle);
updateDistanceSensor(obstacle);
_updateOffboardObstacleDistance(obstacle);
_updateDistanceSensor(obstacle);
//The maximum velocity formula contains a square root, therefore the whole calculation is done with squared norms.
//that way the root does not have to be calculated for every range bin but once at the end.
@ -281,7 +279,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
{
//calculate movement constraints based on range data
Vector2f new_setpoint = original_setpoint;
calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
//warn user if collision prevention starts to interfere
bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
@ -294,7 +292,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
}
_interfering = currently_interfering;
publishConstrainedSetpoint(original_setpoint, new_setpoint);
_publishConstrainedSetpoint(original_setpoint, new_setpoint);
original_setpoint = new_setpoint;
}

60
src/lib/CollisionPrevention/CollisionPrevention.hpp
View File

@ -62,9 +62,18 @@ public:
CollisionPrevention(ModuleParams *parent);
~CollisionPrevention();
/**
* Returs true if Collision Prevention is running
*/
bool is_active() { return _param_mpc_col_prev_d.get() > 0; }
/**
* Computes collision free setpoints
* @param original_setpoint, setpoint before collision prevention intervention
* @param max_speed, maximum xy speed
* @param curr_pos, current vehicle position
* @param curr_vel, current vehicle velocity
*/
void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
@ -89,9 +98,12 @@ private:
(ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly /**< delay of the range measurement data*/
)
void update();
inline float sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset)
/**
* Transforms the sensor orientation into a yaw in the local frame
* @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;
@ -117,14 +129,38 @@ private:
return offset;
}
void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos,
const matrix::Vector2f &curr_vel);
void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
void publishObstacleDistance(obstacle_distance_s &obstacle);
void updateOffboardObstacleDistance(obstacle_distance_s &obstacle);
void updateDistanceSensor(obstacle_distance_s &obstacle);
/**
* Computes collision free setpoints
* @param setpoint, setpoint before collision prevention intervention
* @param curr_pos, current vehicle position
* @param curr_vel, current vehicle velocity
*/
void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos,
const matrix::Vector2f &curr_vel);
/**
* Publishes collision_constraints message
* @param original_setpoint, setpoint before collision prevention intervention
* @param adapted_setpoint, collision prevention adaped setpoint
*/
void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
/**
* Publishes obstacle_distance message with fused data from offboard and from distance sensors
* @param obstacle, obstacle_distance message to be publsihed
*/
void _publishObstacleDistance(obstacle_distance_s &obstacle);
/**
* Updates obstacle distance message with measurement from offboard
* @param obstacle, obstacle_distance message to be updated
*/
void _updateOffboardObstacleDistance(obstacle_distance_s &obstacle);
/**
* Updates obstacle distance message with measurement from distance sensors
* @param obstacle, obstacle_distance message to be updated
*/
void _updateDistanceSensor(obstacle_distance_s &obstacle);
/**
* Publishes vehicle command.
*/
void _publishVehicleCmdDoLoiter();
};