Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 10:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: AC_PosControl: Support Accel only input

Browse Source
This commit is contained in:
Leonard Hall 2021-06-20 15:32:24 +09:30 committed by Andrew Tridgell
parent a546a9ac14
commit d5d7e3d5d3
2 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -481,6 +481,20 @@ void AC_PosControl::init_xy()
_last_update_xy_us = AP_HAL::micros64();
}
/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The time constant also defines the time taken to achieve the maximum acceleration.
void AC_PosControl::input_accel_xy(const Vector3f& accel)
{
// check for ekf xy position reset
handle_ekf_xy_reset();
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy());
shape_accel_xy(accel, _accel_desired, _accel_max_xy_cmss, _tc_xy_s, _dt);
}
/// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
/// The vel is projected forwards in time based on a time step of dt and acceleration accel.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.

7
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -89,6 +89,13 @@ public:
/// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void relax_velocity_controller_xy();
/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The time constant also defines the time taken to achieve the maximum acceleration.
void input_accel_xy(const Vector3f& accel);
/// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.