Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: convert poscontrol to use double position

This commit is contained in:
Andrew Tridgell 2021-06-18 11:19:29 +10:00
parent e6a1cee08f
commit 0af57de50c
2 changed files with 33 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -334,12 +334,12 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// 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 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 input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void AC_PosControl::input_pos_vel_accel_xyz(const Vector3f& pos) void AC_PosControl::input_pos_vel_accel_xyz(const Vector3p& pos)
{ {
// check for ekf xy position reset // check for ekf xy position reset
handle_ekf_xy_reset(); handle_ekf_xy_reset();
handle_ekf_z_reset(); handle_ekf_z_reset();
Vector3f dest_vector = pos - _pos_target; Vector3f dest_vector = (pos - _pos_target).tofloat();
// calculated increased maximum acceleration if over speed // calculated increased maximum acceleration if over speed
float accel_z_cmss = _accel_max_z_cmss; float accel_z_cmss = _accel_max_z_cmss;
@ -359,7 +359,7 @@ void AC_PosControl::input_pos_vel_accel_xyz(const Vector3f& pos)
float vel_max_z_cms = 0.0f; float vel_max_z_cms = 0.0f;
if (is_positive(dest_vector.length_squared()) ) { if (is_positive(dest_vector.length_squared()) ) {
dest_vector.normalize(); dest_vector.normalize();
float dest_vector_xy_length = Vector2f{dest_vector.x, dest_vector.y}.length(); float dest_vector_xy_length = dest_vector.xy().length();
float vel_max_cms = kinematic_limit(dest_vector, _vel_max_xy_cms, _vel_max_up_cms, _vel_max_down_cms); float vel_max_cms = kinematic_limit(dest_vector, _vel_max_xy_cms, _vel_max_up_cms, _vel_max_down_cms);
vel_max_xy_cms = vel_max_cms * dest_vector_xy_length; vel_max_xy_cms = vel_max_cms * dest_vector_xy_length;
@ -367,19 +367,18 @@ void AC_PosControl::input_pos_vel_accel_xyz(const Vector3f& pos)
} }
Vector3f vel; Vector2f vel;
Vector3f accel; Vector2f accel;
shape_pos_vel_accel_xy(pos.xy(), vel.xy(), accel.xy(), _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), shape_pos_vel_accel_xy(pos.xy(), vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(),
vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt); vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt);
float posz = pos.z; float posz = pos.z;
shape_pos_vel_accel(posz, vel.z, accel.z, shape_pos_vel_accel(posz, 0, 0,
_pos_target.z, _vel_desired.z, _accel_desired.z, _pos_target.z, _vel_desired.z, _accel_desired.z,
vel_max_z_cms, _vel_max_down_cms, _vel_max_up_cms, vel_max_z_cms, _vel_max_down_cms, _vel_max_up_cms,
-constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss, -constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
_tc_z_s, _dt); _tc_z_s, _dt);
} }
/// ///
/// Lateral position controller /// Lateral position controller
/// ///
@ -506,7 +505,7 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel)
/// The pos and vel are projected forwards in time based on a time step of dt and acceleration accel. /// 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 function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The function alters the pos and vel to be the kinematic path based on accel /// 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) void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel)
{ {
// check for ekf xy position reset // check for ekf xy position reset
handle_ekf_xy_reset(); handle_ekf_xy_reset();
@ -514,7 +513,7 @@ void AC_PosControl::input_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const V
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy()); update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy());
shape_pos_vel_accel_xy(pos, vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), shape_pos_vel_accel_xy(pos, vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(),
_vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt); _vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt);
update_pos_vel_accel_xy(pos, vel, accel, _dt, Vector2f()); update_pos_vel_accel_xy(pos, vel, accel, _dt, Vector2f());
} }
@ -814,7 +813,9 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float ac
-constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss, -constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
_tc_z_s, _dt); _tc_z_s, _dt);
update_pos_vel_accel(pos, vel, accel, _dt, 0); postype_t posp = pos;
update_pos_vel_accel(posp, vel, accel, _dt, 0);
pos = posp;
} }
/// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm /// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm
@ -850,7 +851,9 @@ void AC_PosControl::update_z_controller()
const float curr_alt = _inav.get_position().z; const float curr_alt = _inav.get_position().z;
// calculate the target velocity correction // calculate the target velocity correction
_vel_target.z = _p_pos_z.update_all(_pos_target.z, curr_alt, _limit.pos_down, _limit.pos_up); float pos_target_zf = _pos_target.z;
_vel_target.z = _p_pos_z.update_all(pos_target_zf, curr_alt, _limit.pos_down, _limit.pos_up);
_pos_target.z = pos_target_zf;
// add feed forward component // add feed forward component
_vel_target.z += _vel_desired.z; _vel_target.z += _vel_desired.z;
@ -908,7 +911,7 @@ void AC_PosControl::update_z_controller()
/// ///
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void AC_PosControl::get_stopping_point_z_cm(float &stopping_point) const void AC_PosControl::get_stopping_point_z_cm(postype_t &stopping_point) const
{ {
const float curr_pos_z = _inav.get_position().z; const float curr_pos_z = _inav.get_position().z;
float curr_vel_z = _inav.get_velocity().z; float curr_vel_z = _inav.get_velocity().z;
@ -937,7 +940,7 @@ float AC_PosControl::get_lean_angle_max_cd() const
} }
/// set position, velocity and acceleration targets /// set position, velocity and acceleration targets
void AC_PosControl::set_pos_vel_accel(const Vector3f& pos, const Vector3f& vel, const Vector3f& accel) void AC_PosControl::set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel, const Vector3f& accel)
{ {
_pos_target = pos; _pos_target = pos;
_vel_desired = vel; _vel_desired = vel;
@ -945,7 +948,7 @@ void AC_PosControl::set_pos_vel_accel(const Vector3f& pos, const Vector3f& vel,
} }
/// set position, velocity and acceleration targets /// set position, velocity and acceleration targets
void AC_PosControl::set_pos_vel_accel_xy(const Vector2f& pos, const Vector2f& vel, const Vector2f& accel) void AC_PosControl::set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel)
{ {
_pos_target.x = pos.x; _pos_target.x = pos.x;
_pos_target.y = pos.y; _pos_target.y = pos.y;
@ -983,10 +986,10 @@ Vector3f AC_PosControl::get_thrust_vector() const
/// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration /// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
/// function does not change the z axis /// function does not change the z axis
void AC_PosControl::get_stopping_point_xy_cm(Vector2f &stopping_point) const void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
{ {
const Vector3f curr_pos = _inav.get_position(); const Vector3f curr_pos = _inav.get_position();
stopping_point = curr_pos.xy(); stopping_point = curr_pos.xy().topostype();
float kP = _p_pos_xy.kP(); float kP = _p_pos_xy.kP();
Vector3f curr_vel = _inav.get_velocity(); Vector3f curr_vel = _inav.get_velocity();
@ -1018,7 +1021,7 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2f &stopping_point) const
/// get_bearing_to_target_cd - get bearing to target position in centi-degrees /// get_bearing_to_target_cd - get bearing to target position in centi-degrees
int32_t AC_PosControl::get_bearing_to_target_cd() const int32_t AC_PosControl::get_bearing_to_target_cd() const
{ {
return get_bearing_cd(_inav.get_position(), _pos_target); return get_bearing_cd(_inav.get_position(), _pos_target.tofloat());
} }
@ -1048,7 +1051,7 @@ void AC_PosControl::standby_xyz_reset()
_pid_accel_z.set_integrator(0.0f); _pid_accel_z.set_integrator(0.0f);
// Set the target position to the current pos. // Set the target position to the current pos.
_pos_target = _inav.get_position(); _pos_target = _inav.get_position().topostype();
// Set _pid_vel_xy integrator and derivative to zero. // Set _pid_vel_xy integrator and derivative to zero.
_pid_vel_xy.reset_filter(); _pid_vel_xy.reset_filter();
@ -1063,7 +1066,7 @@ void AC_PosControl::write_log()
if (is_active_xy()) { if (is_active_xy()) {
float accel_x, accel_y; float accel_x, accel_y;
lean_angles_to_accel_xy(accel_x, accel_y); lean_angles_to_accel_xy(accel_x, accel_y);
AP::logger().Write_PSC(get_pos_target_cm(), _inav.get_position(), get_vel_target_cms(), _inav.get_velocity(), get_accel_target_cmss(), accel_x, accel_y); AP::logger().Write_PSC(get_pos_target_cm().tofloat(), _inav.get_position(), get_vel_target_cms(), _inav.get_velocity(), get_accel_target_cmss(), accel_x, accel_y);
} }
if (is_active_z()) { if (is_active_z()) {

18
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -54,7 +54,7 @@ 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. /// 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 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 kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
void input_pos_vel_accel_xyz(const Vector3f& pos); void input_pos_vel_accel_xyz(const Vector3p& pos);
/// ///
/// Lateral position controller /// Lateral position controller
@ -96,7 +96,7 @@ public:
/// 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 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 kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// The function alters the pos and vel to be the kinematic path based on accel /// 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); void input_pos_vel_accel_xy(Vector2p& 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 // is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
bool is_active_xy() const; bool is_active_xy() const;
@ -190,8 +190,8 @@ public:
/// ///
/// set commanded position (cm), velocity (cm/s) and acceleration (cm/s/s) inputs when the path is created externally. /// set commanded position (cm), velocity (cm/s) and acceleration (cm/s/s) inputs when the path is created externally.
void set_pos_vel_accel(const Vector3f& pos, const Vector3f& vel, const Vector3f& accel); void set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel, const Vector3f& accel);
void set_pos_vel_accel_xy(const Vector2f& pos, const Vector2f& vel, const Vector2f& accel); void set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel);
/// Position /// Position
@ -200,7 +200,7 @@ public:
void set_pos_target_xy_cm(float pos_x, float pos_y) { _pos_target.x = pos_x; _pos_target.y = pos_y; } void set_pos_target_xy_cm(float pos_x, float pos_y) { _pos_target.x = pos_x; _pos_target.y = pos_y; }
/// get_pos_target_cm - returns the position target in NEU cm from home /// get_pos_target_cm - returns the position target in NEU cm from home
const Vector3f& get_pos_target_cm() const { return _pos_target; } const Vector3p& get_pos_target_cm() const { return _pos_target; }
/// set_pos_target_z_cm - set altitude target in cm above home /// set_pos_target_z_cm - set altitude target in cm above home
void set_pos_target_z_cm(float pos_target) { _pos_target.z = pos_target; } void set_pos_target_z_cm(float pos_target) { _pos_target.z = pos_target; }
@ -209,13 +209,13 @@ public:
float get_pos_target_z_cm() const { return _pos_target.z; } float get_pos_target_z_cm() const { return _pos_target.z; }
/// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration /// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void get_stopping_point_xy_cm(Vector2f &stopping_point) const; void get_stopping_point_xy_cm(Vector2p &stopping_point) const;
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void get_stopping_point_z_cm(float &stopping_point) const; void get_stopping_point_z_cm(postype_t &stopping_point) const;
/// get_pos_error_cm - get position error vector between the current and target position /// get_pos_error_cm - get position error vector between the current and target position
const Vector3f get_pos_error_cm() const { return _pos_target - _inav.get_position(); } const Vector3f get_pos_error_cm() const { return (_pos_target - _inav.get_position().topostype()).tofloat(); }
/// get_pos_error_xy_cm - get the length of the position error vector in the xy plane /// get_pos_error_xy_cm - get the length of the position error vector in the xy plane
float get_pos_error_xy_cm() const { return norm(_pos_target.x - _inav.get_position().x, _pos_target.y - _inav.get_position().y); } float get_pos_error_xy_cm() const { return norm(_pos_target.x - _inav.get_position().x, _pos_target.y - _inav.get_position().y); }
@ -396,7 +396,7 @@ protected:
float _yaw_rate_target; // desired yaw rate in centi-degrees per second calculated by position controller float _yaw_rate_target; // desired yaw rate in centi-degrees per second calculated by position controller
// position controller internal variables // position controller internal variables
Vector3f _pos_target; // target location in NEU cm from home Vector3p _pos_target; // target location in NEU cm from home
Vector3f _vel_desired; // desired velocity in NEU cm/s Vector3f _vel_desired; // desired velocity in NEU cm/s
Vector3f _vel_target; // velocity target in NEU cm/s calculated by pos_to_rate step Vector3f _vel_target; // velocity target in NEU cm/s calculated by pos_to_rate step
Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward) Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward)