dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FlightTaskOrbit: smooth yaw like in missions

This commit is contained in:
Matthias Grob 2020-12-16 15:01:02 +01:00 committed by Julian Kent
parent 44606ca872
commit 537ee5b19b
3 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt
View File

@ -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})

11
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -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 &center_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();

5
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp
View File

@ -45,6 +45,7 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/orbit_status.h>
#include <StraightLine.hpp>
#include <SlewRateYaw.hpp>
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<float> _slew_rate_yaw;
uORB::Publication<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise /**< cruise speed for circle approach */
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, /**< cruise speed for circle approach */
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max
)
};