From e191c48d9fa57ada633c03db2a96206627eeeaec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Jun 2021 10:03:09 +1000 Subject: [PATCH] AC_AttitudeControl: cleanup poscontrol comments with input from Leonard --- .../AC_AttitudeControl/AC_PosControl.cpp | 15 +++----------- libraries/AC_AttitudeControl/AC_PosControl.h | 20 ++++--------------- 2 files changed, 7 insertions(+), 28 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 7e2da79efc..bb5687fe69 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -489,8 +489,6 @@ void AC_PosControl::init_xy() /// 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. /// 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_vel_accel_xy(Vector2f& vel, const Vector2f& accel) { // check for ekf xy position reset @@ -507,9 +505,7 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel) /// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// The pos and vel are 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. -/// 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. +/// The function alters the pos and vel to be the kinematic path based on accel void AC_PosControl::input_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel) { // check for ekf xy position reset @@ -750,10 +746,7 @@ void AC_PosControl::init_z() /// input_vel_accel_z - 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. -/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z 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. -/// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing. +/// The function alters the vel to be the kinematic path based on accel void AC_PosControl::input_vel_accel_z(float &vel, const float accel, bool ignore_descent_limit) { // check for ekf z position reset @@ -797,9 +790,7 @@ void AC_PosControl::set_pos_target_z_from_climb_rate_cm(const float vel, bool ig /// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// The pos and vel are 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. -/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z 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. +/// The function alters the pos and vel to be the kinematic path based on accel void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float accel) { // check for ekf z position reset diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 8655c56b18..668c585574 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -54,8 +54,6 @@ public: /// input_pos_vel_accel_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position 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. - /// 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_pos_vel_accel_xyz(const Vector3f& pos); /// @@ -91,17 +89,13 @@ public: /// 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. - /// 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. - /// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. + /// The function alters the vel to be the kinematic path based on accel void input_vel_accel_xy(Vector2f& vel, const Vector2f& accel); /// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position 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. - /// 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. - /// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. + /// The function alters the pos and vel to be the kinematic path based on accel void input_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel); // is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times @@ -163,10 +157,7 @@ public: /// input_vel_accel_z - 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_z 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. - /// The function alters the input velocitiy to be the velocity that the system could reach zero acceleration in the minimum time. - /// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing. + /// The function alters the vel to be the kinematic path based on accel virtual void input_vel_accel_z(float &vel, const float accel, bool ignore_descent_limit); /// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a climb rate in cm/s @@ -176,10 +167,7 @@ public: /// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position 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_z 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. - /// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. + /// The function alters the pos and vel to be the kinematic path based on accel void input_pos_vel_accel_z(float &pos, float &vel, float accel); /// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm