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

mc_pos_control: check if position-control setpoint update succeeded.

Browse Source 
refactor use_obstacle_avoidance method
This commit is contained in:
Dennis Mannhart
2018-10-10 10:29:46 -04:00
committed by Daniel Agar
parent b83b588fc5
commit 2f833a26d1
3 changed files with 24 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+9 -10
View File
@@ -267,7 +267,7 @@ private:
/**
* Overwrite setpoint with waypoint coming from obstacle avoidance
*/
void execute_avoidance_waypoint();
void execute_avoidance_waypoint(vehicle_local_position_setpoint_s &setpoint);
/**
* Publish desired vehicle_trajectory_waypoint
@@ -703,11 +703,14 @@ MulticopterPositionControl::run()
_control.updateConstraints(constraints);
_control.updateState(_states);
if (!use_obstacle_avoidance()) {
_control.updateSetpoint(setpoint);
// adjust setpoints based on avoidance
if (use_obstacle_avoidance()) {
execute_avoidance_waypoint(setpoint);
}
} else {
execute_avoidance_waypoint();
// update position controller setpoints
if (!_control.updateSetpoint(setpoint)) {
warn_rate_limited("Position-Control Setpoint-Update failed");
}
// Generate desired thrust and yaw.
@@ -1076,10 +1079,8 @@ MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlSta
}
void
MulticopterPositionControl::execute_avoidance_waypoint()
MulticopterPositionControl::execute_avoidance_waypoint(vehicle_local_position_setpoint_s &setpoint)
{
vehicle_local_position_setpoint_s setpoint;
setpoint.x = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0];
setpoint.y = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1];
setpoint.z = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2];
@@ -1093,8 +1094,6 @@ MulticopterPositionControl::execute_avoidance_waypoint()
setpoint.yaw = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
setpoint.yawspeed = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
Vector3f(NAN, NAN, NAN).copyTo(setpoint.thrust);
_control.updateSetpoint(setpoint);
}
bool