fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-09 19:29:17 +01:00
parent db1fb6acf7
commit e03585ca06
2 changed files with 31 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -190,13 +190,7 @@ bool FlightTaskOrbit::update()
{
bool ret = true;
_updateTrajectoryBoundaries();
// 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;
_adjustParametersByStick();
if (_is_position_on_circle()) {
if (_in_circle_approach) {
@ -215,7 +209,7 @@ bool FlightTaskOrbit::update()
// update altitude
ret = ret && FlightTaskManualAltitudeSmoothVel::update();
// this generates x / y setpoints
// this generates x, y and yaw setpoints
_generate_circle_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()
{
const Vector2f center2d = Vector2f(_center);

2
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
View File

@ -106,6 +106,8 @@ private:
*/
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 */
void _generate_circle_approach_setpoints();
/** generates xy setpoints to make the vehicle orbit */