Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 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
+16 -23
View File
@@ -424,11 +424,11 @@ private:
bool use_avoidance_velocity_waypoint();
void update_avoidance_waypoints_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 yaw_speed);
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 yaw_speed);
void reset_wp_avoidance_desired();
void reset_avoidance_waypoint_desired();
/**
* Temporary method for flight control compuation
@@ -3304,28 +3304,23 @@ MulticopterPositionControl::task_main()
if (_pos_sp_triplet.current.valid) {
/* 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),
_vel_sp_desired(1),
_vel_sp_desired(1),
NAN, NAN, NAN, _yaw, NAN);
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(2), NAN, NAN, NAN, _yaw, NAN);
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,
_pos_sp_triplet.current.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.current.yaw, NAN);
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);
}
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,
_pos_sp_triplet.next.z,
NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.next.yaw, NAN);
update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_2, _pos_sp_triplet.next.x, _pos_sp_triplet.next.y,
_pos_sp_triplet.next.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.next.yaw, NAN);
}
} else {
update_avoidance_waypoints_desired(trajectory_waypoint_s::POINT_0, _pos(0), _pos(1), _pos(2), _vel_sp_desired(0),
_vel_sp_desired(1),
_vel_sp_desired(2),
NAN, NAN, NAN, _yaw, NAN);
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(2), NAN, NAN, NAN, _yaw, NAN);
}
/* 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);
}
reset_wp_avoidance_desired();
reset_avoidance_waypoint_desired();
} else {
/* 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]);
}
void MulticopterPositionControl::update_avoidance_waypoints_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 yaw_speed)
void MulticopterPositionControl::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 yaw_speed)
{
_traj_wp_avoidance_desired.timestamp = hrt_absolute_time();
_traj_wp_avoidance_desired.type = trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;