Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

FlightTaskOrbit: fix tangential yaw stick input for rotation direction changes

Browse Source
This commit is contained in:
Matthias Grob
2022-03-09 20:22:51 +01:00
parent 74c0ae6e55
commit 18629bb535
2 changed files with 7 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+6 -4
View File
@@ -68,7 +68,8 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
new_absolute_velocity = command.param2;
}
float new_velocity = (new_is_clockwise ? 1.f : -1.f) * new_absolute_velocity;
float new_velocity = signFromBool(new_is_clockwise) * new_absolute_velocity;
_started_clockwise = new_is_clockwise;
_sanitizeParams(new_radius, new_velocity);
_orbit_radius = new_radius;
_orbit_velocity = new_velocity;
@@ -263,8 +264,8 @@ void FlightTaskOrbit::_adjustParametersByStick()
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();
radius -= signFromBool(_started_clockwise) * _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_xy_cruise.get();
velocity += signFromBool(_started_clockwise) * _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_acc_hor.get();
break;
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING:
@@ -339,7 +340,8 @@ void FlightTaskOrbit::_generate_circle_yaw_setpoints()
break;
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
_yaw_setpoint = atan2f(sign(_orbit_velocity) * center_to_position(0), -sign(_orbit_velocity) * center_to_position(1));
_yaw_setpoint = atan2f(signFromBool(_started_clockwise) * center_to_position(0),
-signFromBool(_started_clockwise) * center_to_position(1));
_yawspeed_setpoint = _orbit_velocity / _orbit_radius;
break;
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+1
View File
@@ -124,6 +124,7 @@ 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;
bool _started_clockwise{true};
float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
SlewRateYaw<float> _slew_rate_yaw;