Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: Add monitoring and reporting of forward accel saturation

Browse Source
This commit is contained in:
Paul Riseborough 2023-09-12 18:36:24 +10:00 committed by Andrew Tridgell
parent e3ce7d966b
commit 75789bd94d
2 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -656,6 +656,13 @@ void AC_PosControl::update_xy_controller()
if (!limit_accel_xy(_vel_desired.xy(), _accel_target.xy(), accel_max)) { if (!limit_accel_xy(_vel_desired.xy(), _accel_target.xy(), accel_max)) {
// _accel_target was not limited so we can zero the xy limit vector // _accel_target was not limited so we can zero the xy limit vector
_limit_vector.xy().zero(); _limit_vector.xy().zero();
} else {
// Check for pitch limiting in the forward direction
const float accel_fwd_unlimited = _limit_vector.x * _ahrs.cos_yaw() + _limit_vector.y * _ahrs.sin_yaw();
const float pitch_target_unlimited = accel_to_angle(- MIN(accel_fwd_unlimited, accel_max) * 0.01f) * 100;
const float accel_fwd_limited = _accel_target.x * _ahrs.cos_yaw() + _accel_target.y * _ahrs.sin_yaw();
const float pitch_target_limited = accel_to_angle(- accel_fwd_limited * 0.01f) * 100;
_fwd_pitch_is_limited = is_negative(pitch_target_unlimited) && pitch_target_unlimited < pitch_target_limited;
} }
// update angle targets that will be passed to stabilize controller // update angle targets that will be passed to stabilize controller

5
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -394,6 +394,9 @@ public:
// get earth-frame Z-axis acceleration with gravity removed in cm/s/s with +ve being up // get earth-frame Z-axis acceleration with gravity removed in cm/s/s with +ve being up
float get_z_accel_cmss() const { return -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f; } float get_z_accel_cmss() const { return -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f; }
/// returns true when the forward pitch demand is limited by the maximum allowed tilt
bool get_fwd_pitch_is_limited() const { return _fwd_pitch_is_limited; }
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
protected: protected:
@ -462,6 +465,8 @@ protected:
Vector3f _accel_target; // acceleration target in NEU cm/s/s 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 Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited
bool _fwd_pitch_is_limited; // true when the forward pitch demand is being limited to meet acceleration limits
float _pos_offset_target_z; // vertical position offset target, frame NEU in cm relative to the EKF origin float _pos_offset_target_z; // vertical position offset target, frame NEU in cm relative to the EKF origin
float _pos_offset_z; // vertical position offset, frame NEU in cm relative to the EKF origin float _pos_offset_z; // vertical position offset, frame NEU in cm relative to the EKF origin
float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step