From 537ee5b19baaa8f6b63e6705b7bb88cbdde85b01 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 16 Dec 2020 15:01:02 +0100 Subject: [PATCH] FlightTaskOrbit: smooth yaw like in missions --- src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt | 2 +- src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp | 11 +++++++++-- src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp | 5 ++++- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt b/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt index 3ec96bb7a6..d925f87c9e 100644 --- a/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskOrbit FlightTaskOrbit.cpp ) -target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel) +target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel SlewRate) target_include_directories(FlightTaskOrbit PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index 723eed1069..8de9c54022 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -162,6 +162,8 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set _center = Vector2f(_position); _center(0) -= _r; _initial_heading = _yaw; + _slew_rate_yaw.setForcedValue(_yaw); + _slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get())); // need a valid position and velocity ret = ret && PX4_ISFINITE(_position(0)) @@ -196,6 +198,9 @@ bool FlightTaskOrbit::update() generate_circle_yaw_setpoints(center_to_position); } + // Apply yaw smoothing + _yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime); + // publish information to UI sendTelemetry(); @@ -204,16 +209,18 @@ bool FlightTaskOrbit::update() void FlightTaskOrbit::generate_circle_approach_setpoints(const Vector2f ¢er_to_position) { + const Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero(); + if (_circle_approach_line.isEndReached()) { // calculate target point on circle and plan a line trajectory - const Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero(); 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.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)); + // follow the planned line and switch to orbiting once the circle is reached _circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint); _in_circle_approach = !_circle_approach_line.isEndReached(); diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp index 04cb6fcd48..bed54e5388 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp @@ -45,6 +45,7 @@ #include #include #include +#include class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel { @@ -111,10 +112,12 @@ private: /** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */ int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER; float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ + SlewRateYaw _slew_rate_yaw; uORB::Publication _orbit_status_pub{ORB_ID(orbit_status)}; DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_xy_cruise /**< cruise speed for circle approach */ + (ParamFloat) _param_mpc_xy_cruise, /**< cruise speed for circle approach */ + (ParamFloat) _param_mpc_yawrauto_max ) };