mirror of https://github.com/ArduPilot/ardupilot
141 lines
8.4 KiB
C
141 lines
8.4 KiB
C
#pragma once
|
|
|
|
/*
|
|
common controller helper functions
|
|
*/
|
|
|
|
// update_vel_accel projects the velocity, vel, forward in time based on a time step of dt and acceleration of accel.
|
|
// update_vel_accel - single axis projection.
|
|
void update_vel_accel(float& vel, float accel, float dt, float limit);
|
|
void update_vel_accel_z(Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit);
|
|
|
|
// update_vel_accel projects the velocity, vel, forward in time based on a time step of dt and acceleration of accel.
|
|
// update_vel_accel - single axis projection.
|
|
void update_pos_vel_accel(float& pos, float& vel, float accel, float dt, float limit);
|
|
void update_pos_vel_accel_z(Vector3f& pos, Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit);
|
|
|
|
// update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs.
|
|
void update_vel_accel(Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit);
|
|
void update_vel_accel_xy(Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit);
|
|
|
|
// update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs.
|
|
void update_pos_vel_accel(Vector2f& pos, Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit);
|
|
void update_pos_vel_accel_xy(Vector3f& pos, Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit);
|
|
|
|
/* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration.
|
|
The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
|
|
The kinematic path is constrained by :
|
|
acceleration limits - accel_min, accel_max,
|
|
time constant - tc.
|
|
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 time constant must be positive.
|
|
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
|
|
*/
|
|
void shape_accel(float accel_input, float& accel,
|
|
float accel_min, float accel_max,
|
|
float tc, float dt);
|
|
|
|
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
|
|
float accel_max, float tc, float dt);
|
|
|
|
/* shape_vel calculates a jerk limited path from the current velocity and acceleration to an input velocity.
|
|
The function takes the current velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
|
|
The kinematic path is constrained by :
|
|
velocity limits - vel_min, vel_max,
|
|
acceleration limits - accel_min, accel_max,
|
|
time constant - tc.
|
|
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 time constant must be positive.
|
|
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
|
|
*/
|
|
void shape_vel_accel(float vel_input, float accel_input,
|
|
float vel, float& accel,
|
|
float vel_min, float vel_max,
|
|
float accel_min, float accel_max,
|
|
float tc, float dt);
|
|
|
|
void shape_vel_accel_z(const Vector3f& vel_input, const Vector3f& accel_input,
|
|
const Vector3f& vel, Vector3f& accel,
|
|
float vel_min, float vel_max,
|
|
float accel_min, float accel_max,
|
|
float tc, float dt);
|
|
|
|
/* shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
|
|
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:
|
|
vel_max : maximum velocity
|
|
accel_max : maximum acceleration
|
|
tc : 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 time constant must be positive.
|
|
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
|
|
This function operates on the x and y axis of both Vector2f or Vector3f inputs.
|
|
The accel_max limit can be removed by setting it to zero.
|
|
*/
|
|
void shape_vel_accel_xy(Vector2f vel_input, const Vector2f& accel_input,
|
|
const Vector2f& vel, Vector2f& accel, float vel_max, float accel_max, float tc, float dt);
|
|
|
|
void shape_vel_accel_xy(const Vector3f& vel_input, const Vector3f& accel_input,
|
|
const Vector3f& vel, Vector3f& accel, float vel_max, float accel_max, float tc, float dt);
|
|
|
|
/* shape_pos_vel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
|
|
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 :
|
|
maximum velocity - vel_max,
|
|
maximum acceleration - accel_max,
|
|
time constant - tc.
|
|
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 time constant must be positive.
|
|
The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
|
|
*/
|
|
void shape_pos_vel_accel(float pos_input, float vel_input, float accel_input,
|
|
float pos, float vel, float& accel,
|
|
float vel_correction_max, float vel_min, float vel_max,
|
|
float accel_min, float accel_max, float tc, float dt);
|
|
|
|
void shape_pos_vel_accel_z(const Vector3f& pos_input, const Vector3f& vel_input, const Vector3f& accel_input,
|
|
const Vector3f& pos, const Vector3f& vel, Vector3f& accel,
|
|
float vel_correction_max, float vel_min, float vel_max,
|
|
float accel_min, float accel_max, float tc, float dt);
|
|
|
|
/* shape_pos_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
|
|
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:
|
|
vel_max : maximum velocity
|
|
accel_max : maximum acceleration
|
|
tc : 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 time constant must be positive.
|
|
The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
|
|
This function operates only on the x and y axis of the Vector2f or Vector3f inputs.
|
|
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
|
|
*/
|
|
void shape_pos_vel_accel_xy(const Vector2f& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
|
|
const Vector2f& pos, const Vector2f& vel, Vector2f& accel,
|
|
float vel_correction_max, float vel_max, float accel_max, float tc, float dt);
|
|
|
|
void shape_pos_vel_accel_xy(const Vector3f& pos_input, const Vector3f& vel_input, const Vector3f& accel_input,
|
|
const Vector3f& pos, const Vector3f& vel, Vector3f& accel,
|
|
float vel_max, float vel_correction_max, float accel_max, float tc, float dt);
|
|
|
|
// proportional controller with piecewise sqrt sections to constrain second derivative
|
|
float sqrt_controller(float error, float p, float second_ord_lim, float dt);
|
|
|
|
// proportional controller with piecewise sqrt sections to constrain second derivative
|
|
Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, float dt);
|
|
|
|
// inverse of the sqrt controller. calculates the input (aka error) to the sqrt_controller required to achieve a given output
|
|
float inv_sqrt_controller(float output, float p, float D_max);
|
|
|
|
// calculate the stopping distance for the square root controller based deceleration path
|
|
float stopping_distance(float velocity, float p, float accel_max);
|
|
|
|
// calculate the maximum acceleration or velocity in a given direction
|
|
// based on horizontal and vertical limits.
|
|
float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg);
|