Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: add lean angle max

Browse Source
This commit is contained in:
Leonard Hall 2017-12-16 23:36:51 +10:30 committed by Randy Mackay
parent 0f3645b061
commit e5bc2b26fe
2 changed files with 40 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -168,6 +168,15 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
// @User: Advanced
AP_SUBGROUPINFO(_pid_vel_xy, "_VELXY_", 6, AC_PosControl, AC_PID_2D),
// @Param: _ANGLE_MAX
// @DisplayName: Position Control Angle Max
// @Description: Maximum lean angle autopilot can request. Set to zero to use ANGLE_MAX parameter value
// @Units: deg
// @Range: 0 45
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("_ANGLE_MAX", 7, AC_PosControl, _lean_angle_max, 0.0f),
AP_GROUPEND
};
@ -761,6 +770,15 @@ bool AC_PosControl::is_active_xy() const
return ((AP_HAL::millis() - _last_update_xy_ms) <= POSCONTROL_ACTIVE_TIMEOUT_MS);
}
/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float AC_PosControl::get_lean_angle_max_cd() const
{
if (is_zero(_lean_angle_max)) {
return _attitude_control.lean_angle_max();
}
return _lean_angle_max * 100.0f;
}
/// init_xy_controller - initialise the xy controller
/// sets target roll angle, pitch angle and I terms based on vehicle current lean angles
/// should be called once whenever significant changes to the position target are made
@ -1049,21 +1067,28 @@ void AC_PosControl::run_xy_controller(float dt, float ekfNavVelGainScaler)
// the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles
float accel_right, accel_forward;
// limit acceleration using maximum lean angles
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), _attitude_control.lean_angle_max());
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
float accel_max = MIN(GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)), POSCONTROL_ACCEL_XY_MAX);
_limit.accel_xy = limit_vector_length(_accel_target.x, _accel_target.y, accel_max);
// update angle targets that will be passed to stabilize controller
accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
}
// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{
float accel_right, accel_forward;
// rotate accelerations into body forward-right frame
accel_forward = _accel_target.x*_ahrs.cos_yaw() + _accel_target.y*_ahrs.sin_yaw();
accel_right = -_accel_target.x*_ahrs.sin_yaw() + _accel_target.y*_ahrs.cos_yaw();
accel_forward = accel_x_cmss*_ahrs.cos_yaw() + accel_y_cmss*_ahrs.sin_yaw();
accel_right = -accel_x_cmss*_ahrs.sin_yaw() + accel_y_cmss*_ahrs.cos_yaw();
// update angle targets that will be passed to stabilize controller
_pitch_target = atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI);
float cos_pitch_target = cosf(_pitch_target*M_PI/18000);
_roll_target = atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI);
pitch_target = atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI);
float cos_pitch_target = cosf(pitch_target*M_PI/18000);
roll_target = atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI);
}
// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s

7
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -160,6 +160,9 @@ public:
/// xy position controller
///
/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float get_lean_angle_max_cd() const;
/// init_xy_controller - initialise the xy controller
/// sets target roll angle, pitch angle and I terms based on vehicle current lean angles
/// should be called once whenever significant changes to the position target are made
@ -281,6 +284,9 @@ public:
const Vector3f& get_vel_target() const { return _vel_target; }
const Vector3f& get_accel_target() const { return _accel_target; }
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
void accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const;
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
void lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const;
@ -364,6 +370,7 @@ protected:
// parameters
AP_Float _accel_xy_filt_hz; // XY acceleration filter cutoff frequency
AP_Float _lean_angle_max; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
AC_P _p_pos_z;
AC_P _p_vel_z;
AC_PID _pid_accel_z;