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

FlightTaskOrbit: move start_to_circle to more local scope & const reference parameters

Browse Source
This commit is contained in:
Matthias Grob
2020-07-21 09:44:21 +02:00
parent b8b46f8493
commit abf6f4fa0a
2 changed files with 11 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
+8 -9
View File
@@ -179,11 +179,10 @@ bool FlightTaskOrbit::update()
setRadius(r); setRadius(r);
setVelocity(v); setVelocity(v);
Vector2f center_to_position = Vector2f(_position) - _center; const Vector2f center_to_position = Vector2f(_position) - _center;
Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
if (_in_circle_approach) { if (_in_circle_approach) {
generate_circle_approach_setpoints(start_to_circle); generate_circle_approach_setpoints(center_to_position);
} else { } else {
generate_circle_setpoints(center_to_position); generate_circle_setpoints(center_to_position);
@@ -196,13 +195,13 @@ bool FlightTaskOrbit::update()
return ret; return ret;
} }
void FlightTaskOrbit::generate_circle_approach_setpoints(Vector2f start_to_circle) void FlightTaskOrbit::generate_circle_approach_setpoints(const Vector2f &center_to_position)
{ {
if (_circle_approach_line.isEndReached()) { if (_circle_approach_line.isEndReached()) {
// calculate target point on circle and plan a line trajectory // calculate target point on circle and plan a line trajectory
Vector2f closest_circle_point = Vector2f(_position) + start_to_circle; const Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2)); const Vector2f closest_circle_point = Vector2f(_position) + start_to_circle;
const Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2));
_circle_approach_line.setLineFromTo(_position, target); _circle_approach_line.setLineFromTo(_position, target);
_circle_approach_line.setSpeed(_param_mpc_xy_cruise.get()); _circle_approach_line.setSpeed(_param_mpc_xy_cruise.get());
_yaw_setpoint = atan2f(start_to_circle(1), start_to_circle(0)); _yaw_setpoint = atan2f(start_to_circle(1), start_to_circle(0));
@@ -213,7 +212,7 @@ void FlightTaskOrbit::generate_circle_approach_setpoints(Vector2f start_to_circl
_in_circle_approach = !_circle_approach_line.isEndReached(); _in_circle_approach = !_circle_approach_line.isEndReached();
} }
void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position) void FlightTaskOrbit::generate_circle_setpoints(const Vector2f &center_to_position)
{ {
// xy velocity to go around in a circle // xy velocity to go around in a circle
Vector2f velocity_xy(-center_to_position(1), center_to_position(0)); Vector2f velocity_xy(-center_to_position(1), center_to_position(0));
@@ -228,7 +227,7 @@ void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position)
_acceleration_setpoint.xy() = -center_to_position.unit_or_zero() * _v * _v / _r; _acceleration_setpoint.xy() = -center_to_position.unit_or_zero() * _v * _v / _r;
} }
void FlightTaskOrbit::generate_circle_yaw_setpoints(Vector2f center_to_position) void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f &center_to_position)
{ {
switch (_yaw_behaviour) { switch (_yaw_behaviour) {
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING: case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING:
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp
+3 -3
View File
@@ -89,11 +89,11 @@ protected:
private: private:
/** generates setpoints to smoothly reach the closest point on the circle when starting from far away */ /** generates setpoints to smoothly reach the closest point on the circle when starting from far away */
void generate_circle_approach_setpoints(matrix::Vector2f &center_to_position); void generate_circle_approach_setpoints(const matrix::Vector2f &center_to_position);
/** generates xy setpoints to make the vehicle orbit */ /** generates xy setpoints to make the vehicle orbit */
void generate_circle_setpoints(matrix::Vector2f &center_to_position); void generate_circle_setpoints(const matrix::Vector2f &center_to_position);
/** generates yaw setpoints to control the vehicle's heading */ /** generates yaw setpoints to control the vehicle's heading */
void generate_circle_yaw_setpoints(matrix::Vector2f &center_to_position); void generate_circle_yaw_setpoints(const matrix::Vector2f &center_to_position);
float _r = 0.f; /**< radius with which to orbit the target */ float _r = 0.f; /**< radius with which to orbit the target */
float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */ float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */