fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 14:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskOrbit: remap stick input when vehicle faces tangential

Browse Source
This commit is contained in:
Matthias Grob
2022-03-23 10:51:45 +01:00
parent db1fb6acf7
commit e03585ca06
2 changed files with 31 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+29 -8
View File
@@ -190,13 +190,7 @@ bool FlightTaskOrbit::update()
{ {
bool ret = true; bool ret = true;
_updateTrajectoryBoundaries(); _updateTrajectoryBoundaries();
_adjustParametersByStick();
// stick input adjusts parameters within a fixed time frame
float radius = _orbit_radius - _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_xy_cruise.get();
float velocity = _orbit_velocity - _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_acc_hor.get();
_sanitizeParams(radius, velocity);
_orbit_radius = radius;
_orbit_velocity = velocity;
if (_is_position_on_circle()) { if (_is_position_on_circle()) {
if (_in_circle_approach) { if (_in_circle_approach) {
@@ -215,7 +209,7 @@ bool FlightTaskOrbit::update()
// update altitude // update altitude
ret = ret && FlightTaskManualAltitudeSmoothVel::update(); ret = ret && FlightTaskManualAltitudeSmoothVel::update();
// this generates x / y setpoints // this generates x, y and yaw setpoints
_generate_circle_setpoints(); _generate_circle_setpoints();
_generate_circle_yaw_setpoints(); _generate_circle_yaw_setpoints();
} }
@@ -262,6 +256,33 @@ bool FlightTaskOrbit::_is_position_on_circle() const
} }
void FlightTaskOrbit::_adjustParametersByStick()
{
float radius = _orbit_radius;
float velocity = _orbit_velocity;
switch (_yaw_behaviour) {
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
radius -= _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_xy_cruise.get();
velocity += _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_acc_hor.get();
break;
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING:
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:
default:
// stick input adjusts parameters within a fixed time frame
radius -= _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_xy_cruise.get();
velocity -= _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_acc_hor.get();
break;
}
_sanitizeParams(radius, velocity);
_orbit_radius = radius;
_orbit_velocity = velocity;
}
void FlightTaskOrbit::_generate_circle_approach_setpoints() void FlightTaskOrbit::_generate_circle_approach_setpoints()
{ {
const Vector2f center2d = Vector2f(_center); const Vector2f center2d = Vector2f(_center);
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+2
View File
@@ -106,6 +106,8 @@ private:
*/ */
bool _is_position_on_circle() const; bool _is_position_on_circle() const;
/** Adjusts radius and speed according to stick input */
void _adjustParametersByStick();
/** 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(); void _generate_circle_approach_setpoints();
/** generates xy setpoints to make the vehicle orbit */ /** generates xy setpoints to make the vehicle orbit */