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 c8079a318c
commit 66186e5221
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 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.
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
handle_ekf_xy_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
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;
if (is_positive(dest_vector.length_squared()) ) {
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);
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;
Vector3f accel;
shape_pos_vel_accel_xy(pos.xy(), vel.xy(), accel.xy(), _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);
Vector2f vel;
Vector2f accel;
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);
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,
vel_max_z_cms, _vel_max_down_cms, _vel_max_up_cms,
-constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
_tc_z_s, _dt);
}
///
/// 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 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
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
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());
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());
}
@ -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,
_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
@ -850,7 +851,9 @@ void AC_PosControl::update_z_controller()
const float curr_alt = _inav.get_position().z;
// 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
_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
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;
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
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;
_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
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.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
/// 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();
stopping_point = curr_pos.xy();
stopping_point = curr_pos.xy().topostype();
float kP = _p_pos_xy.kP();
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
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);
// 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.
_pid_vel_xy.reset_filter();
@ -1063,7 +1066,7 @@ void AC_PosControl::write_log()
if (is_active_xy()) {
float 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()) {

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.
/// 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.
void input_pos_vel_accel_xyz(const Vector3f& pos);
void input_pos_vel_accel_xyz(const Vector3p& pos);
///
/// 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 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
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
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.
void set_pos_vel_accel(const Vector3f& 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(const Vector3p& pos, const Vector3f& vel, const Vector3f& accel);
void set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel);
/// 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; }
/// 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
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; }
/// 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
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
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
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
// 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_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)