Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

mc_pos_control: fix desired trajectory waypoint. (#10372)

Browse Source 
- The first waypoint contains the vehicle current position and the desired velocity setpoint
This commit is contained in:
Martina Rivizzigno
2018-08-31 23:28:15 +02:00
committed by Daniel Agar
parent 1b6e933176
commit 21f6ca4a1f
1 changed files with 13 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+13 -6
View File
@@ -256,7 +256,7 @@ private:
* point2: current triplet only if in auto mode
* @param states current vehicle state
*/
void update_avoidance_waypoint_desired(PositionControlStates &states);
void update_avoidance_waypoint_desired(PositionControlStates &states, vehicle_local_position_setpoint_s &setpoint);
/**
* Check whether or not use the obstacle avoidance waypoint
@@ -635,7 +635,7 @@ MulticopterPositionControl::task_main()
* point_0 contains the current position with the desired velocity
* point_1 contains _pos_sp_triplet.current if valid
*/
update_avoidance_waypoint_desired(_states);
update_avoidance_waypoint_desired(_states, setpoint);
vehicle_constraints_s constraints = _flight_tasks.getConstraints();
@@ -1004,7 +1004,8 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
}
void
MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlStates &states)
MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlStates &states,
vehicle_local_position_setpoint_s &setpoint)
{
if (MPC_OBS_AVOID.get()) {
const vehicle_trajectory_waypoint_s traj_wp_desired_new = _flight_tasks.getAvoidanceWaypoint();
@@ -1015,8 +1016,13 @@ MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlSta
_traj_wp_avoidance_desired.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].acceleration);
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0] = setpoint.vx;
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1] = setpoint.vy;
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2] = setpoint.vz;
states.acceleration.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].acceleration);
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = states.yaw;
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed = NAN;
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
@@ -1037,7 +1043,8 @@ MulticopterPositionControl::execute_avoidance_waypoint()
setpoint.vx = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0];
setpoint.vy = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1];
setpoint.vz = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2];;
setpoint.vz = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2];
setpoint.acc_x = setpoint.acc_y = setpoint.acc_z = NAN;
setpoint.yaw = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;