Skip to content

Commit

Permalink
Bring z up to date with xy
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall
lthall committed Sep 19, 2024
1 parent 7e62e82 commit 3ae916b
Show file tree
Hide file tree
Showing 3 changed files with 185 additions and 78 deletions.
226 changes: 164 additions & 62 deletions libraries/AC_AttitudeControl/AC_PosControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -365,13 +365,15 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
/// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The jerk limit 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, float pos_offset_z, float pos_offset_z_buffer)
void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_target_z, float pos_offset_z_buffer)
{
// Terrain following velocity scalar must be calculated before we remove the position offset
const float offset_z_scaler = pos_offset_z_scaler(pos_offset_z, pos_offset_z_buffer);
const float offset_z_scaler = pos_offset_z_scaler(pos_offset_target_z, pos_offset_z_buffer);
set_posvelaccel_offset_target_z_cm(pos_offset_target_z, 0.0, 0.0);

// remove offsets including terrain offset for flat earth assumption
remove_offsets_xy();
remove_offsets_z();

// calculated increased maximum acceleration and jerk if over speed
const float overspeed_gain = calculate_overspeed_gain();
Expand Down Expand Up @@ -413,10 +415,11 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float

// update the position, velocity and acceleration offsets
update_xy_offsets();
update_pos_offset_z(pos_offset_z);
update_z_offsets();

// add back offsets including terrain offset
add_offsets_xy();
add_offsets_z();
}


Expand Down Expand Up @@ -551,7 +554,7 @@ void AC_PosControl::init_xy_controller()

// initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
update_xy_offsets();
set_posvelaccel_offsets_xy_cm(_pos_offset_target.xy(), _vel_offset_target, _accel_offset_target);
set_posvelaccel_offsets_xy_cm(_pos_offset_target.xy(), _vel_offset_target.xy(), _accel_offset_target.xy());

// initialise z_controller time out
_last_update_xy_ticks = AP::scheduler().ticks32();
Expand Down Expand Up @@ -628,42 +631,64 @@ void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const V
add_offsets_xy();
}

/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
void AC_PosControl::stop_pos_xy_stabilisation()
{
_pos_target.xy() = _inav.get_position_xy_cm().topostype();
}

/// stop_vel_xy_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system
void AC_PosControl::stop_vel_xy_stabilisation()
{
_pos_target.xy() = _inav.get_position_xy_cm().topostype();

const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
_vel_desired.xy() = curr_vel;
// with zero position error _vel_target = _vel_desired
_vel_target.xy() = curr_vel;

// initialise I terms from lean angles
_pid_vel_xy.reset_filter();
_pid_vel_xy.reset_I();
}

/// update the horizontal position and velocity offsets
/// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target, _vel_offset_target, _accel_offset_target)
void AC_PosControl::update_xy_offsets()
{
// return immediately if offsets have never been set
if (_xy_offset_type == XYOffsetType::NONE) {
if (_xy_offset_type == OffsetType::NONE) {
return;
}

// check for offset target timeout
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_xy_offset_type = XYOffsetType::POS_VEL_ACCEL;
if (now_ms - _posvelaccel_offset_target_xy_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_xy_offset_type = OffsetType::POS_VEL_ACCEL;
_pos_offset_target.zero();
_vel_offset_target.zero();
_accel_offset_target.zero();
}

// update position, velocity, accel offsets for this iteration
update_pos_vel_accel_xy(_pos_offset_target.xy(), _vel_offset_target.xy(), _accel_offset_target.xy(), _dt, Vector2f(), Vector2f(), Vector2f());
update_pos_vel_accel_xy(_pos_offset.xy(), _vel_offset.xy(), _accel_offset.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());

switch (_xy_offset_type) {

case XYOffsetType::NONE:
case OffsetType::NONE:
break;

case XYOffsetType::POS_VEL_ACCEL:
case OffsetType::POS_VEL_ACCEL:
// input shape horizontal position, velocity and acceleration offsets
shape_pos_vel_accel_xy(_pos_offset_target.xy(), _vel_offset_target, _accel_offset_target,
shape_pos_vel_accel_xy(_pos_offset_target.xy(), _vel_offset_target.xy(), _accel_offset_target.xy(),
_pos_offset.xy(), _vel_offset.xy(), _accel_offset.xy(),
_vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, false);
break;

case XYOffsetType::VEL_ACCEL:
case OffsetType::VEL_ACCEL:
// input shape horizontal velocity and acceleration offsets
shape_vel_accel_xy(_vel_offset_target, _accel_offset_target,
shape_vel_accel_xy(_vel_offset_target.xy(), _accel_offset_target.xy(),
_vel_offset.xy(), _accel_offset.xy(),
_accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, false);

Expand All @@ -673,27 +698,6 @@ void AC_PosControl::update_xy_offsets()
}
}

/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
void AC_PosControl::stop_pos_xy_stabilisation()
{
_pos_target.xy() = _inav.get_position_xy_cm().topostype();
}

/// stop_vel_xy_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system
void AC_PosControl::stop_vel_xy_stabilisation()
{
_pos_target.xy() = _inav.get_position_xy_cm().topostype();

const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
_vel_desired.xy() = curr_vel;
// with zero position error _vel_target = _vel_desired
_vel_target.xy() = curr_vel;

// initialise I terms from lean angles
_pid_vel_xy.reset_filter();
_pid_vel_xy.reset_I();
}

// is_active_xy - returns true if the xy position controller has been run in the previous loop
bool AC_PosControl::is_active_xy() const
{
Expand Down Expand Up @@ -886,11 +890,9 @@ void AC_PosControl::init_z_controller()
_accel_target.z = _accel_desired.z;
_pid_accel_z.reset_filter();

// initialise vertical offsets
_pos_offset_target.z = 0.0;
_pos_offset.z = 0.0;
_vel_offset.z = 0.0;
_accel_offset.z = 0.0;
// initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
update_z_offsets();
set_posvelaccel_offsets_z_cm(_pos_offset_target.z, _vel_offset_target.z, _accel_offset_target.z);

// Set accel PID I term based on the current throttle
// Remove the expected P term due to _accel_desired.z being constrained to _accel_max_z_cmss
Expand Down Expand Up @@ -955,7 +957,7 @@ void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel)
input_vel_accel_z(vel_temp, 0.0);

// update the vertical position, velocity and acceleration offsets
update_pos_offset_z(_pos_offset_target.z);
update_z_offsets();

// add terrain offsets
_pos_target.z += _pos_offset.z;
Expand Down Expand Up @@ -1010,19 +1012,56 @@ void AC_PosControl::set_alt_target_with_slew(float pos)
input_pos_vel_accel_z(pos, 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)
/// update_z_offsets - updates the vertical offsets used by terrain following
void AC_PosControl::update_z_offsets()
{
postype_t p_offset_z = _pos_offset.z;
// return immediately if offsets have never been set
if (_z_offset_type == OffsetType::NONE) {
return;
}

// check for offset target timeout
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_z_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_z_offset_type = OffsetType::POS_VEL_ACCEL;
_pos_offset_target.xy().zero();
_vel_offset_target.xy().zero();
_accel_offset_target.xy().zero();
}

// update position, velocity, accel offsets for this iteration
postype_t p_offset_z = _pos_offset_target.z;
update_pos_vel_accel(p_offset_z, _vel_offset_target.z, _accel_offset_target.z, _dt, 0.0, 0.0, 0.0);
_pos_offset_target.z = p_offset_z;
p_offset_z = _pos_offset.z;
update_pos_vel_accel(p_offset_z, _vel_offset.z, _accel_offset.z, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error());
_pos_offset.z = p_offset_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,
get_max_speed_down_cms(), get_max_speed_up_cms(),
-get_max_accel_z_cmss(), get_max_accel_z_cmss(),
_jerk_max_z_cmsss, _dt, false);
switch (_z_offset_type) {

case OffsetType::NONE:
break;

case OffsetType::POS_VEL_ACCEL:
// input shape horizontal position, velocity and acceleration offsets
shape_pos_vel_accel(_pos_offset_target.z, _vel_offset_target.z, _accel_offset_target.z,
_pos_offset.z, _vel_offset.z, _accel_offset.z,
get_max_speed_down_cms(), get_max_speed_up_cms(),
-get_max_accel_z_cmss(), get_max_accel_z_cmss(),
_jerk_max_z_cmsss, _dt, false);
break;

case OffsetType::VEL_ACCEL:
// input shape horizontal velocity and acceleration offsets
shape_vel_accel(_vel_offset_target.z, _accel_offset_target.z,
_vel_offset.z, _accel_offset.z,
-get_max_accel_z_cmss(), get_max_accel_z_cmss(),
_jerk_max_z_cmsss, _dt, false);

// set the position target offset to the current position offset
_pos_offset_target.z = _pos_offset.z;
break;
}
}

// is_active_z - returns true if the z position controller has been run in the previous loop
Expand Down Expand Up @@ -1135,7 +1174,9 @@ void AC_PosControl::set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel,
_vel_desired = vel;
_accel_desired = accel;
update_xy_offsets();
update_z_offsets();
add_offsets_xy();
add_offsets_z();
}

/// set position, velocity and acceleration targets
Expand All @@ -1148,6 +1189,16 @@ void AC_PosControl::set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& ve
add_offsets_xy();
}

/// set position, velocity and acceleration targets
void AC_PosControl::set_pos_vel_accel_z(float pos, float vel, float accel)
{
_pos_target.z = pos;
_vel_desired.z = vel;
_accel_desired.z = accel;
update_z_offsets();
add_offsets_z();
}

/// returns the position target (not including offsets), frame NEU in cm relative to the EKF origin
Vector3p AC_PosControl::get_pos_target_cm() const
{
Expand Down Expand Up @@ -1204,6 +1255,20 @@ void AC_PosControl::set_posvelaccel_offsets_xy_cm(const Vector2p& pos_offset_xy_
_accel_offset.xy() = accel_offset_xy_cmss;
}

/// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
void AC_PosControl::set_posvelaccel_offsets_z_cm(float pos_offset_z_cm, float vel_offset_z_cms, float accel_offset_z_cmss)
{
// set position offset to target
_pos_offset.z = pos_offset_z_cm;

// set velocity offset to target
_vel_offset.z = vel_offset_z_cms;

// set acceleration offset to target
_accel_offset.z = accel_offset_z_cmss;
}

// remove offsets from the position velocity and acceleration targets
void AC_PosControl::remove_offsets_xy()
{
Expand All @@ -1213,6 +1278,15 @@ void AC_PosControl::remove_offsets_xy()
_accel_desired.xy() -= _accel_offset.xy();
}

// remove offsets from the position velocity and acceleration targets
void AC_PosControl::remove_offsets_z()
{
// remove offsets
_pos_target.z -= _pos_offset.z;
_vel_desired.z -= _vel_offset.z;
_accel_desired.z -= _accel_offset.z;
}

// add offsets back to the position velocity and acceleration targets
void AC_PosControl::add_offsets_xy()
{
Expand All @@ -1222,6 +1296,15 @@ void AC_PosControl::add_offsets_xy()
_accel_desired.xy() += _accel_offset.xy();
}

// add offsets back to the position velocity and acceleration targets
void AC_PosControl::add_offsets_z()
{
// add back offsets
_pos_target.z += _pos_offset.z;
_vel_desired.z += _vel_offset.z;
_accel_desired.z += _accel_offset.z;
}

#if AP_SCRIPTING_ENABLED
// add an additional offset to vehicle's target position, velocity and acceleration
// units are m, m/s and m/s/s in NED frame
Expand Down Expand Up @@ -1257,30 +1340,49 @@ bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &v
void AC_PosControl::set_posvelaccel_offset_target_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss)
{
// set position offset target
_xy_offset_type = XYOffsetType::POS_VEL_ACCEL;
_xy_offset_type = OffsetType::POS_VEL_ACCEL;
_pos_offset_target.xy() = pos_offset_target_xy_cm;

// set velocity offset target and limit to 1/2 maximum
_vel_offset_target = vel_offset_target_xy_cms;
_vel_offset_target.limit_length(get_max_speed_xy_cms() * 0.5);

// set acceleration offset target and limit to 1/2 maximum
_accel_offset_target = accel_offset_target_xy_cmss;
_accel_offset_target.limit_length(get_max_accel_xy_cmss() * 0.5);
_vel_offset_target.xy() = vel_offset_target_xy_cms;
_accel_offset_target.xy() = accel_offset_target_xy_cmss;

// record time of update so we can detect timeouts
_posvelaccel_offset_target_ms = AP_HAL::millis();
_posvelaccel_offset_target_xy_ms = AP_HAL::millis();
}

/// set the horizontal velocity and acceleration offset targets in cm/s and cm/s/s in NE frame
/// these must be set every 3 seconds (or less) or they will timeout and return to zero
void AC_PosControl::set_velaccel_offset_target_xy_cms(const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss)
{
// re-use set_posvelaccel_offset_target_z_cm but with zero position offset target
set_posvelaccel_offset_target_xy_cm(_pos_offset_target.xy(), vel_offset_target_xy_cms, accel_offset_target_xy_cmss);

// override offset type
_xy_offset_type = OffsetType::VEL_ACCEL;
}

/// set the vertical position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame
/// these must be set every 3 seconds (or less) or they will timeout and return to zero
void AC_PosControl::set_posvelaccel_offset_target_z_cm(float pos_offset_target_z_cm, float vel_offset_target_z_cms, float accel_offset_target_z_cmss)
{
// set position offset target
_z_offset_type = OffsetType::POS_VEL_ACCEL;
_pos_offset_target.z = pos_offset_target_z_cm;
_vel_offset_target.z = vel_offset_target_z_cms;
_accel_offset_target.z = accel_offset_target_z_cmss;

// record time of update so we can detect timeouts
_posvelaccel_offset_target_z_ms = AP_HAL::millis();
}

/// set the vertical velocity and acceleration offset targets in cm/s and cm/s/s in NE frame
/// these must be set every 3 seconds (or less) or they will timeout and return to zero
void AC_PosControl::set_velaccel_offset_target_z_cms(float vel_offset_target_z_cms, float accel_offset_target_z_cmss)
{
// re-use set_posvelaccel_offset_target_xy_cm but with zero position offset target
set_posvelaccel_offset_target_xy_cm(Vector2p(), vel_offset_target_xy_cms, accel_offset_target_xy_cmss);
set_posvelaccel_offset_target_z_cm(_pos_offset_target.z, vel_offset_target_z_cms, accel_offset_target_z_cmss);

// override offset type
_xy_offset_type = XYOffsetType::VEL_ACCEL;
_z_offset_type = OffsetType::VEL_ACCEL;
}

// returns the NED target acceleration vector for attitude control
Expand Down Expand Up @@ -1395,8 +1497,8 @@ void AC_PosControl::write_log()
_accel_desired.y, accel_target.y, accel_y);

// log offsets if they have ever been used
if (_xy_offset_type != XYOffsetType::NONE) {
Write_PSCO(_pos_offset_target.xy(), _pos_offset.xy(), _vel_offset_target, _vel_offset.xy(), _accel_offset_target, _accel_offset.xy());
if (_xy_offset_type != OffsetType::NONE) {
Write_PSCO(_pos_offset_target.xy(), _pos_offset.xy(), _vel_offset_target.xy(), _vel_offset.xy(), _accel_offset_target.xy(), _accel_offset.xy());
}
}

Expand Down
Loading

0 comments on commit 3ae916b

Please sign in to comment.