forked from Archive/PX4-Autopilot
FlightTaskOrbit: smooth yaw like in missions
This commit is contained in:
parent
44606ca872
commit
537ee5b19b
|
@ -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})
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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
|
||||
)
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue