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: Add terain following to guided

Browse Source
This commit is contained in:
Leonard Hall 2021-06-26 23:19:12 +09:30 committed by Andrew Tridgell
parent 97f2ecd06e
commit 9d8fdf3e85
2 changed files with 59 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -334,9 +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_xyz(const Vector3p& pos)
void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z)
{
Vector3f dest_vector = (pos - _pos_target).tofloat();
// remove terrain offsets for flat earth assumption
_pos_target.z -= _pos_offset_z;
_vel_desired.z -= _vel_offset_z;
_accel_desired.z -= _accel_offset_z;
// calculated increased maximum acceleration if over speed
float accel_z_cmss = _accel_max_z_cmss;
@ -349,31 +352,41 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos)
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy());
// adjust desired alt if motors have not hit their limits
// adjust desired altitude if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z);
float vel_max_xy_cms = _vel_max_xy_cms;
// calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos
float vel_max_xy_cms = 0.0f;
float vel_max_z_cms = 0.0f;
Vector3f dest_vector = (pos - _pos_target).tofloat();
if (is_positive(dest_vector.length_squared()) ) {
dest_vector.normalize();
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;
vel_max_z_cms = vel_max_cms * dest_vector.z;
vel_max_z_cms = fabsf(vel_max_cms * dest_vector.z);
}
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, 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);
// update the vertical position, velocity and acceleration offsets
update_pos_offset_z(pos_offset_z);
// add terrain offsets
_pos_target.z += _pos_offset_z;
_vel_desired.z += _vel_offset_z;
_accel_desired.z += _accel_offset_z;
}
///
@ -743,6 +756,11 @@ void AC_PosControl::init_z()
_accel_target.z = -(curr_accel.z + GRAVITY_MSS) * 100.0f;
_pid_accel_z.reset_filter();
// initialise vertical offsets
_pos_offset_z = 0.0;
_vel_offset_z = 0.0;
_accel_offset_z = 0.0;
// initialise ekf z reset handler
init_ekf_z_reset();
@ -806,7 +824,7 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float ac
accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms;
}
// adjust desired alt if motors have not hit their limits
// adjust desired altitude if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z);
shape_pos_vel_accel(pos, vel, accel,
@ -829,6 +847,21 @@ void AC_PosControl::set_alt_target_with_slew(const float& pos)
input_pos_vel_accel_z(posf, zero, 0);
}
/// update_pos_offset_z - updates the vertical offsets used by terrain following
void AC_PosControl::update_pos_offset_z(float pos_offset_z)
{
postype_t posp_z = _pos_offset_z;
update_pos_vel_accel(posp_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f));
_pos_offset_z = posp_z;
// input shape the terrain offset
shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f,
_pos_offset_z, _vel_offset_z, _accel_offset_z,
0.0f, get_max_speed_down_cms(), get_max_speed_up_cms(),
-get_max_accel_z_cmss(), get_max_accel_z_cmss(), _tc_z_s, _dt);
}
// is_active_z - returns true if the z position controller has been run in the previous 5 loop times
bool AC_PosControl::is_active_z() const
{
@ -857,7 +890,9 @@ void AC_PosControl::update_z_controller()
const float curr_alt = _inav.get_position().z;
// calculate the target velocity correction
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
@ -868,6 +903,7 @@ void AC_PosControl::update_z_controller()
const Vector3f& curr_vel = _inav.get_velocity();
_accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel.z, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
// add feed forward component
_accel_target.z += _accel_desired.z;
// Acceleration Controller

20
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -57,7 +57,7 @@ public:
/// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
/// 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_xyz(const Vector3p& pos);
void input_pos_xyz(const Vector3p& pos, float pos_offset_z);
///
/// Lateral position controller
@ -184,6 +184,9 @@ public:
/// using the default position control kinimatic path.
void set_alt_target_with_slew(const float& pos);
/// update_pos_offset_z - updates the vertical offsets used by terrain following
void update_pos_offset_z(float pos_offset);
// is_active_z - returns true if the z position controller has been run in the previous 5 loop times
bool is_active_z() const;
@ -215,7 +218,7 @@ public:
/// 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; }
/// get_pos_target_z_cm - get desired altitude (in cm above home)
/// get_pos_target_z_cm - get target altitude (in cm above home)
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
@ -264,6 +267,12 @@ public:
const Vector3f& get_accel_target_cmss() const { return _accel_target; }
/// Offset
/// set_pos_offset_z_cm - set altitude offset in cm above home
void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset_z = pos_offset_z; }
/// Outputs
/// get desired roll and pitch to be passed to the attitude controller
@ -413,10 +422,13 @@ protected:
Vector3f _accel_target; // acceleration target in NEU cm/s/s
Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited
Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set
float _pos_offset_z; // vertical position offset in NEU cm from home
float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step
float _accel_offset_z; // vertical acceleration offset in NEU cm/s/s
// ekf reset handling
uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset
uint32_t _ekf_z_reset_ms; // system time of last recorded ekf altitude reset
uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset
uint32_t _ekf_z_reset_ms; // system time of last recorded ekf altitude reset
// high vibration handling
bool _vibe_comp_enabled; // true when high vibration compensation is on