Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

mc_pos_control: uses consistent naming for desired waypoints

Browse Source
This commit is contained in:
Martina
2018-04-06 08:50:34 +02:00
committed by Daniel Agar
parent 272d7ca4cd
commit a11c6235fe
1 changed files with 16 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+15 -22
View File
@@ -424,11 +424,11 @@ private:
bool use_avoidance_velocity_waypoint(); bool use_avoidance_velocity_waypoint();
void update_avoidance_waypoints_desired(const int point_number, const float x, const float y, const float z, void update_avoidance_waypoint_desired(const int point_number, const float x, const float y, const float z,
const float vx, const float vy, const float vz, const float ax, const float ay, const float az, const float yaw, const float vx, const float vy, const float vz, const float ax, const float ay, const float az,
const float yaw_speed); const float yaw, const float yaw_speed);
void reset_wp_avoidance_desired(); void reset_avoidance_waypoint_desired();
/** /**
* Temporary method for flight control compuation * Temporary method for flight control compuation
@@ -3304,28 +3304,23 @@ MulticopterPositionControl::task_main()
if (_pos_sp_triplet.current.valid) { if (_pos_sp_triplet.current.valid) {
/* point_0 containes the current position with the desired velocity */ /* point_0 containes the current position with the desired velocity */
update_avoidance_waypoints_desired(trajectory_waypoint_s::POINT_0, _pos(0), _pos(1), _pos(2), _vel_sp_desired(0), update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_0, _pos(0), _pos(1), _pos(2), _vel_sp_desired(0),
_vel_sp_desired(1), _vel_sp_desired(1), _vel_sp_desired(2), NAN, NAN, NAN, _yaw, NAN);
_vel_sp_desired(1),
NAN, NAN, NAN, _yaw, NAN);
if (_pos_sp_triplet.current.valid) { if (_pos_sp_triplet.current.valid) {
update_avoidance_waypoints_desired(trajectory_waypoint_s::POINT_1, _pos_sp_triplet.current.x, _pos_sp_triplet.current.y, update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_1, _pos_sp_triplet.current.x, _pos_sp_triplet.current.y,
_pos_sp_triplet.current.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.current.yaw, NAN); _pos_sp_triplet.current.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.current.yaw, NAN);
} }
if (_pos_sp_triplet.next.valid) { if (_pos_sp_triplet.next.valid) {
update_avoidance_waypoints_desired(trajectory_waypoint_s::POINT_2, _pos_sp_triplet.next.x, _pos_sp_triplet.next.y, update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_2, _pos_sp_triplet.next.x, _pos_sp_triplet.next.y,
_pos_sp_triplet.next.z, _pos_sp_triplet.next.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.next.yaw, NAN);
NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.next.yaw, NAN);
} }
} else { } else {
update_avoidance_waypoints_desired(trajectory_waypoint_s::POINT_0, _pos(0), _pos(1), _pos(2), _vel_sp_desired(0), update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_0, _pos(0), _pos(1), _pos(2), _vel_sp_desired(0),
_vel_sp_desired(1), _vel_sp_desired(1), _vel_sp_desired(2), NAN, NAN, NAN, _yaw, NAN);
_vel_sp_desired(2),
NAN, NAN, NAN, _yaw, NAN);
} }
/* publish local position setpoint */ /* publish local position setpoint */
@@ -3344,7 +3339,7 @@ MulticopterPositionControl::task_main()
_traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(trajectory_waypoint_desired), &_traj_wp_avoidance_desired); _traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(trajectory_waypoint_desired), &_traj_wp_avoidance_desired);
} }
reset_wp_avoidance_desired(); reset_avoidance_waypoint_desired();
} else { } else {
/* position controller disabled, reset setpoints */ /* position controller disabled, reset setpoints */
@@ -3521,11 +3516,9 @@ MulticopterPositionControl::use_avoidance_velocity_waypoint()
&& PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::VZ]); && PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::VZ]);
} }
void MulticopterPositionControl::update_avoidance_waypoints_desired(const int point_number, const float x, void MulticopterPositionControl::update_avoidance_waypoint_desired(const int point_number, const float x,
const float y, const float y, const float z, const float vx, const float vy, const float vz, const float ax,
const float z, const float ay, const float az, const float yaw, const float yaw_speed)
const float vx, const float vy, const float vz, const float ax, const float ay, const float az, const float yaw,
const float yaw_speed)
{ {
_traj_wp_avoidance_desired.timestamp = hrt_absolute_time(); _traj_wp_avoidance_desired.timestamp = hrt_absolute_time();
_traj_wp_avoidance_desired.type = trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; _traj_wp_avoidance_desired.type = trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;