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 1d33fa2 commit 338cfc4
Show file tree
Hide file tree
Showing 3 changed files with 135 additions and 55 deletions.
158 changes: 117 additions & 41 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,29 +631,6 @@ void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const V
add_offsets_xy();
}

/// 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()
{
// 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) {
_pos_offset_target.xy().zero();
_vel_offset_target.zero();
_accel_offset_target.zero();
}

// update position, velocity, accel offsets for this iteration
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());

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

update_pos_vel_accel_xy(_pos_offset_target.xy(), _vel_offset_target, _accel_offset_target, _dt, Vector2f(), Vector2f(), Vector2f());
}

/// 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()
{
Expand All @@ -672,6 +652,30 @@ void AC_PosControl::stop_vel_xy_stabilisation()
_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()
{
// check for offset target timeout
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_xy_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_pos_offset_target.xy().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());

// input shape horizontal position, velocity and acceleration offsets
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);

update_pos_vel_accel_xy(_pos_offset_target.xy(), _vel_offset_target.xy(), _accel_offset_target.xy(), _dt, Vector2f(), Vector2f(), Vector2f());
}

// 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 @@ -864,11 +868,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 @@ -933,7 +935,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 @@ -988,19 +990,32 @@ 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()
{
// 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) {
_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.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,
// 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);

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;
}

// is_active_z - returns true if the z position controller has been run in the previous loop
Expand Down Expand Up @@ -1113,7 +1128,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 @@ -1126,6 +1143,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 @@ -1182,6 +1209,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 @@ -1191,6 +1232,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 @@ -1200,6 +1250,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 @@ -1229,13 +1288,30 @@ void AC_PosControl::set_posvelaccel_offset_target_xy_cm(const Vector2p& pos_offs
_pos_offset_target.xy() = pos_offset_target_xy_cm;

// set velocity offset target
_vel_offset_target = vel_offset_target_xy_cms;
_vel_offset_target.xy() = vel_offset_target_xy_cms;

// set acceleration offset target
_accel_offset_target.xy() = accel_offset_target_xy_cmss;

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

/// 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, const float accel_offset_target_z_cmss)
{
// set position offset target
_pos_offset_target.z = pos_offset_target_z_cm;

// set velocity offset target
_vel_offset_target.z = vel_offset_target_z_cms;

// set acceleration offset target
_accel_offset_target = accel_offset_target_xy_cmss;
_accel_offset_target.z = accel_offset_target_z_cmss;

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

// returns the NED target acceleration vector for attitude control
Expand Down Expand Up @@ -1347,7 +1423,7 @@ void AC_PosControl::write_log()

// log offsets if they have ever been used
if (!_pos_offset_target.xy().is_zero()) {
Write_PSCO(_pos_offset_target.xy(), _pos_offset.xy(), _vel_offset_target, _vel_offset.xy(), _accel_offset_target, _accel_offset.xy());
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
26 changes: 15 additions & 11 deletions libraries/AC_AttitudeControl/AC_PosControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,15 +131,15 @@ class AC_PosControl
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output = true);

// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
bool is_active_xy() const;

/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
void stop_pos_xy_stabilisation();

/// 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 stop_vel_xy_stabilisation();

// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
bool is_active_xy() const;

/// update_xy_controller - runs the horizontal position controller correcting position, velocity and acceleration errors.
/// Position and velocity errors are converted to velocity and acceleration targets using PID objects
/// Desired velocity and accelerations are added to these corrections as they are calculated
Expand Down Expand Up @@ -227,9 +227,6 @@ class AC_PosControl
/// using the default position control kinematic path.
void set_alt_target_with_slew(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;

Expand All @@ -248,6 +245,7 @@ class AC_PosControl
/// 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 Vector3p& pos, const Vector3f& vel, const Vector3f& accel);
void set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel);
void set_pos_vel_accel_z(float pos, float vel, float accel);


/// Position
Expand Down Expand Up @@ -323,6 +321,7 @@ class AC_PosControl
/// set the horizontal 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 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);
void set_posvelaccel_offset_target_z_cm(float pos_offset_target_z_cm, float vel_offset_target_z_cms, float accel_offset_target_z_cmss);

/// get the position, velocity or acceleration offets in cm from EKF origin in NEU frame
const Vector3p& get_pos_offset_target_cm() const { return _pos_offset_target; }
Expand Down Expand Up @@ -453,18 +452,22 @@ class AC_PosControl
// calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected
float calculate_overspeed_gain();

/// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
/// set the 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
/// note that this sets the actual offsets, not the offset targets
void set_posvelaccel_offsets_xy_cm(const Vector2p& pos_offset_xy_cm, const Vector2f& vel_offset_xy_cms, const Vector2f& accel_offset_xy_cmss);
void set_posvelaccel_offsets_z_cm(float pos_offset_xy_cm, float vel_offset_xy_cms, float accel_offset_xy_cmss);

// remove and add back offsets from the position velocity and acceleration targets
void remove_offsets_xy();
void remove_offsets_z();
void add_offsets_xy();
void add_offsets_z();

/// update the horizontal position and velocity offsets
/// update the position and velocity offsets
/// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target or _vel_offset_target)
void update_xy_offsets();
void update_z_offsets();

/// initialise and check for ekf position resets
void init_ekf_xy_reset();
Expand Down Expand Up @@ -522,11 +525,12 @@ class AC_PosControl
// offset handling variables
Vector3p _pos_offset_target; // position offset target in cm relative to the EKF origin in NEU frame
Vector3p _pos_offset; // position offset in cm from the EKF origin in NEU frame. this offset moves towards _pos_offset_target
Vector2f _vel_offset_target; // velocity offset target in cm/s in NEU frame
Vector3f _vel_offset_target; // velocity offset target in cm/s in NEU frame
Vector3f _vel_offset; // velocity offset in NEU cm/s calculated by pos_to_rate step. this offset moves towards _vel_offset_target
Vector2f _accel_offset_target; // acceleration offset target in cm/s/s in NEU frame
Vector3f _accel_offset_target; // acceleration offset target in cm/s/s in NEU frame
Vector3f _accel_offset; // acceleration offset in NEU cm/s/s
uint32_t _posvelaccel_offset_target_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
uint32_t _posvelaccel_offset_target_xy_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
uint32_t _posvelaccel_offset_target_z_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)

// ekf reset handling
uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset
Expand Down
6 changes: 3 additions & 3 deletions libraries/AC_WPNav/AC_WPNav.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -466,13 +466,13 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
const float offset_z_scaler = _pos_control.pos_offset_z_scaler(terr_offset, get_terrain_margin() * 100.0);

// input shape the horizontal and terrain offsets
_pos_control.update_pos_offset_z(terr_offset);
_pos_control.set_posvelaccel_offset_target_z_cm(terr_offset, 0.0, 0.0);

// get position controller's position offset (post input shaping) so it can be used in position error calculation
const Vector3p& psc_pos_offset_target = _pos_control.get_pos_offset_target_cm();
const Vector3p& psc_pos_offset = _pos_control.get_pos_offset_cm();

// get current position and adjust altitude to origin and destination's frame (i.e. _frame)
const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{float(psc_pos_offset_target.x), float(psc_pos_offset_target.y), terr_offset};
const Vector3f &curr_pos = _inav.get_position_neu_cm() - psc_pos_offset.tofloat();
Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();

Expand Down

0 comments on commit 338cfc4

Please sign in to comment.