diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 20311cb37c..00a9f31e21 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -38,9 +38,10 @@ #include -void PositionSmoothing::generateSetpoints( +void PositionSmoothing::_generateSetpoints( const Vector3f &position, const Vector3f(&waypoints)[3], + bool is_single_waypoint, const Vector3f &feedforward_velocity, float delta_time, bool force_zero_velocity_setpoint, @@ -49,7 +50,7 @@ void PositionSmoothing::generateSetpoints( Vector3f velocity_setpoint{0.f, 0.f, 0.f}; if (!force_zero_velocity_setpoint) { - velocity_setpoint = _generateVelocitySetpoint(position, waypoints, feedforward_velocity); + velocity_setpoint = _generateVelocitySetpoint(position, waypoints, is_single_waypoint, feedforward_velocity); } out_setpoints.unsmoothed_velocity = velocity_setpoint; @@ -184,6 +185,7 @@ const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve } const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3], + bool is_single_waypoint, const Vector3f &feedforward_velocity_setpoint) { // Interface: A valid position setpoint generates a velocity target using conservative motion constraints. @@ -201,13 +203,13 @@ const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &posi Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(), _trajectory[2].getCurrentPosition()); - - const Vector3f u_pos_traj_to_dest((_getCrossingPoint(position, waypoints) - pos_traj).unit_or_zero()); + const Vector3f crossing_point = is_single_waypoint ? target : _getCrossingPoint(position, waypoints); + const Vector3f u_pos_traj_to_dest{(crossing_point - pos_traj).unit_or_zero()}; float xy_speed = _getMaxXYSpeed(waypoints); const float z_speed = _getMaxZSpeed(waypoints); - if (_isTurning(target)) { + if (!is_single_waypoint && _isTurning(target)) { // Limit speed during a turn xy_speed = math::min(_max_speed_previous, xy_speed); @@ -235,7 +237,8 @@ const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &posi // Get various path specific vectors Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition()); - Vector2f pos_traj_to_dest_xy = Vector2f(_getCrossingPoint(position, waypoints)) - pos_traj; + Vector2f crossing_point = is_single_waypoint ? Vector2f(target) : Vector2f(_getCrossingPoint(position, waypoints)); + Vector2f pos_traj_to_dest_xy = crossing_point - pos_traj; Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero()); float xy_speed = _getMaxXYSpeed(waypoints); diff --git a/src/lib/motion_planning/PositionSmoothing.hpp b/src/lib/motion_planning/PositionSmoothing.hpp index f09e2c8d64..73886d0124 100644 --- a/src/lib/motion_planning/PositionSmoothing.hpp +++ b/src/lib/motion_planning/PositionSmoothing.hpp @@ -68,7 +68,7 @@ public: /** * @brief Generates new setpoints for jerk, acceleration, velocity and position - * to reach the given waypoint smoothly from current position. + * to reach the given waypoint triplet smoothly from current position * * @param position Current position of the vehicle * @param waypoints 0: Past waypoint, 1: target, 2: Target after next target @@ -77,14 +77,44 @@ public: * @param force_zero_velocity_setpoint Force vehicle to stop. Generate trajectory that ends with still vehicle. * @param out_setpoints Output of the generated setpoints */ - void generateSetpoints( + inline void generateSetpoints( const Vector3f &position, const Vector3f(&waypoints)[3], const Vector3f &feedforward_velocity, float delta_time, bool force_zero_velocity_setpoint, PositionSmoothingSetpoints &out_setpoints - ); + ) + { + _generateSetpoints(position, waypoints, false, feedforward_velocity, delta_time, force_zero_velocity_setpoint, + out_setpoints); + } + + /** + * @brief Generates new setpoints for jerk, acceleration, velocity and position + * to reach the given waypoint smoothly from current position. + * + * @param position Current position of the vehicle + * @param waypoint desired waypoint + * @param feedforward_velocity FF velocity + * @param delta_time Time since last invocation of the function + * @param force_zero_velocity_setpoint Force vehicle to stop. Generate trajectory that ends with still vehicle. + * @param out_setpoints Output of the generated setpoints + */ + inline void generateSetpoints( + const Vector3f &position, + const Vector3f &waypoint, + const Vector3f &feedforward_velocity, + float delta_time, + bool force_zero_velocity_setpoint, + PositionSmoothingSetpoints &out_setpoints + ) + { + Vector3f waypoints[3] = {waypoint, waypoint, waypoint}; + _generateSetpoints(position, waypoints, true, feedforward_velocity, delta_time, force_zero_velocity_setpoint, + out_setpoints); + } + /** * @brief Reset internal state to the given values @@ -394,7 +424,19 @@ private: /* Internal functions */ bool _isTurning(const Vector3f &target) const; + + void _generateSetpoints( + const Vector3f &position, + const Vector3f(&waypoints)[3], + bool is_single_waypoint, + const Vector3f &feedforward_velocity, + float delta_time, + bool force_zero_velocity_setpoint, + PositionSmoothingSetpoints &out_setpoints + ); + const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3], + bool is_single_waypoint, const Vector3f &feedforward_velocity_setpoint); const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const; const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const; diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 931212b09d..a614fb2c87 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -282,9 +282,7 @@ void FlightTaskOrbit::_generate_circle_approach_setpoints() const Vector3f target_circle_point{closest_point_on_circle(0), closest_point_on_circle(1), _center(2)}; PositionSmoothing::PositionSmoothingSetpoints out_setpoints; - _position_smoothing.generateSetpoints(_position, { - _circle_approach_start_position, target_circle_point, target_circle_point - }, + _position_smoothing.generateSetpoints(_position, target_circle_point, {0.f, 0.f, 0.f}, _deltatime, false, out_setpoints); _yaw_setpoint = atan2f(position_to_center_xy(1), position_to_center_xy(0));