From 0e124d0be5ea3ca0f697d17a2e1b4a94959c2de3 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 7 Feb 2017 19:49:55 -0500 Subject: [PATCH] AC_PosControl: Add AC_PosControl_Sub class --- libraries/AC_AttitudeControl/AC_PosControl.h | 6 +- .../AC_AttitudeControl/AC_PosControl_Sub.cpp | 92 +++++++++++++++++++ .../AC_AttitudeControl/AC_PosControl_Sub.h | 39 ++++++++ 3 files changed, 134 insertions(+), 3 deletions(-) create mode 100644 libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp create mode 100644 libraries/AC_AttitudeControl/AC_PosControl_Sub.h diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index e2fcc44ca9..51cb495308 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -115,14 +115,14 @@ public: /// actual position target will be moved no faster than the speed_down and speed_up /// target will also be stopped if the motors hit their limits or leash length is exceeded /// set force_descend to true during landing to allow target to move low enough to slow the motors - void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend); + virtual void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend); /// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward /// should be called continuously (with dt set to be the expected time between calls) /// actual position target will be moved no faster than the speed_down and speed_up /// target will also be stopped if the motors hit their limits or leash length is exceeded /// set force_descend to true during landing to allow target to move low enough to slow the motors - void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend); + virtual void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend); /// add_takeoff_climb_rate - adjusts alt target up or down using a climb rate in cm/s /// should be called continuously (with dt set to be the expected time between calls) @@ -291,7 +291,7 @@ public: static const struct AP_Param::GroupInfo var_info[]; -private: +protected: // general purpose flags struct poscontrol_flags { diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp new file mode 100644 index 0000000000..ac35222dbc --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp @@ -0,0 +1,92 @@ +#include "AC_PosControl_Sub.h" + +AC_PosControl_Sub::AC_PosControl_Sub(const AP_AHRS_View& ahrs, const AP_InertialNav& inav, + const AP_Motors& motors, AC_AttitudeControl& attitude_control, + AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z, + AC_P& p_pos_xy, AC_PI_2D& pi_vel_xy) : + AC_PosControl(ahrs, inav, motors, attitude_control, p_pos_z, p_vel_z, pid_accel_z, p_pos_xy, pi_vel_xy), + _alt_max(0.0f), + _alt_min(0.0f) +{} + +/// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s +/// should be called continuously (with dt set to be the expected time between calls) +/// actual position target will be moved no faster than the speed_down and speed_up +/// target will also be stopped if the motors hit their limits or leash length is exceeded +void AC_PosControl_Sub::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend) +{ + // adjust desired alt if motors have not hit their limits + // To-Do: add check of _limit.pos_down? + if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) { + _pos_target.z += climb_rate_cms * dt; + } + + // do not let target alt get above limit + if (_alt_max < 100 && _pos_target.z > _alt_max) { + _pos_target.z = _alt_max; + _limit.pos_up = true; + } + + // do not let target alt get below limit + if (_alt_min < 0 && _alt_min < _alt_max && _pos_target.z < _alt_min) { + _pos_target.z = _alt_min; + _limit.pos_down = true; + } + + // do not use z-axis desired velocity feed forward + // vel_desired set to desired climb rate for reporting and land-detector + _flags.use_desvel_ff_z = false; + _vel_desired.z = climb_rate_cms; +} + +/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward +/// should be called continuously (with dt set to be the expected time between calls) +/// actual position target will be moved no faster than the speed_down and speed_up +/// target will also be stopped if the motors hit their limits or leash length is exceeded +/// set force_descend to true during landing to allow target to move low enough to slow the motors +void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend) +{ + // calculated increased maximum acceleration if over speed + float accel_z_cms = _accel_z_cms; + if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) { + accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms; + } + if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms)) { + accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms; + } + accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f); + + // jerk_z is calculated to reach full acceleration in 1000ms. + float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO; + + float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z)); + + _accel_last_z_cms += jerk_z * dt; + _accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms); + + float vel_change_limit = _accel_last_z_cms * dt; + _vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit); + _flags.use_desvel_ff_z = true; + + // adjust desired alt if motors have not hit their limits + // To-Do: add check of _limit.pos_down? + if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) { + _pos_target.z += _vel_desired.z * dt; + } + + // do not let target alt get above limit + if (_alt_max < 100 && _pos_target.z > _alt_max) { + _pos_target.z = _alt_max; + _limit.pos_up = true; + // decelerate feed forward to zero + _vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit); + } + + // do not let target alt get below limit + if (_alt_min < 0 && _alt_min < _alt_max && _pos_target.z < _alt_min) { + _pos_target.z = _alt_min; + _limit.pos_down = true; + // decelerate feed forward to zero + _vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit); + } +} diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.h b/libraries/AC_AttitudeControl/AC_PosControl_Sub.h new file mode 100644 index 0000000000..98dad4cc32 --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.h @@ -0,0 +1,39 @@ +#pragma once + +#include "AC_PosControl.h" + +class AC_PosControl_Sub : public AC_PosControl { +public: + AC_PosControl_Sub(const AP_AHRS_View & ahrs, const AP_InertialNav& inav, + const AP_Motors& motors, AC_AttitudeControl& attitude_control, + AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z, + AC_P& p_pos_xy, AC_PI_2D& pi_vel_xy); + + /// set_alt_max - sets maximum altitude above the ekf origin in cm + /// only enforced when set_alt_target_from_climb_rate is used + /// set to zero to disable limit + void set_alt_max(float alt) { _alt_max = alt; } + + /// set_alt_min - sets the minimum altitude (maximum depth) in cm + /// only enforced when set_alt_target_from_climb_rate is used + /// set to zero to disable limit + void set_alt_min(float alt) { _alt_min = alt; } + + /// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s + /// should be called continuously (with dt set to be the expected time between calls) + /// actual position target will be moved no faster than the speed_down and speed_up + /// target will also be stopped if the motors hit their limits or leash length is exceeded + /// set force_descend to true during landing to allow target to move low enough to slow the motors + void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend) override; + + /// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward + /// should be called continuously (with dt set to be the expected time between calls) + /// actual position target will be moved no faster than the speed_down and speed_up + /// target will also be stopped if the motors hit their limits or leash length is exceeded + /// set force_descend to true during landing to allow target to move low enough to slow the motors + void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend) override; + +private: + float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence + float _alt_min; // min altitude - should be updated from the main code with altitude limit from fence +};