From 338cfc49de19d3dd4bbb16ea2780bb34d8ac8974 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 19 Sep 2024 16:42:55 +0930 Subject: [PATCH] Bring z up to date with xy --- .../AC_AttitudeControl/AC_PosControl.cpp | 158 +++++++++++++----- libraries/AC_AttitudeControl/AC_PosControl.h | 26 +-- libraries/AC_WPNav/AC_WPNav.cpp | 6 +- 3 files changed, 135 insertions(+), 55 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index a5aeaf8f100c37..2a2ae5e67b3453 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -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(); @@ -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(); } @@ -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(); @@ -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() { @@ -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 { @@ -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 @@ -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; @@ -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 @@ -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 @@ -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 { @@ -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() { @@ -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() { @@ -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 @@ -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 @@ -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()); } } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 24303e406948f8..766bc40d008390 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -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 @@ -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; @@ -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 @@ -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; } @@ -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(); @@ -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 diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 9d477bf4713357..988859aa9ea770 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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();