From a1f1dd805962d24f383af9abcce6524e8d3b3a37 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 2 May 2014 12:08:18 +0900 Subject: [PATCH] AC_PosControl: add is_active_z method Consolidated z-axis timeout checks to save 4bytes of RAM Added POS_CONTROL_ACTIVE_TIMEOUT_MS to make timeout consistent --- .../AC_AttitudeControl/AC_PosControl.cpp | 39 ++++++++++++------- libraries/AC_AttitudeControl/AC_PosControl.h | 12 ++++-- 2 files changed, 35 insertions(+), 16 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0b53181ec9..f8fed31373 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -35,9 +35,8 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, _pid_rate_lat(pid_rate_lat), _pid_rate_lon(pid_rate_lon), _dt(POSCONTROL_DT_10HZ), - _last_update_ms(0), - _last_update_rate_ms(0), - _last_update_accel_ms(0), + _last_update_xy_ms(0), + _last_update_z_ms(0), _step(0), _speed_down_cms(POSCONTROL_SPEED_DOWN), _speed_up_cms(POSCONTROL_SPEED_UP), @@ -65,6 +64,8 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, _flags.recalc_leash_xy = true; _flags.recalc_leash_z = true; _flags.keep_xy_I_terms = false; + _flags.reset_rate_to_accel_z = true; + _flags.reset_accel_to_throttle = true; } /// @@ -181,9 +182,23 @@ void AC_PosControl::init_takeoff() } } +// is_active_z - returns true if the z-axis position controller has been run very recently +bool AC_PosControl::is_active_z() const +{ + return ((hal.scheduler->millis() - _last_update_z_ms) <= POSCONTROL_ACTIVE_TIMEOUT_MS); +} + /// update_z_controller - fly to altitude in cm above home void AC_PosControl::update_z_controller() { + // check time since last cast + uint32_t now = hal.scheduler->millis(); + if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) { + _flags.reset_rate_to_accel_z = true; + _flags.reset_accel_to_throttle = true; + } + _last_update_z_ms = now; + // check if leash lengths need to be recalculated calc_leash_length_z(); @@ -257,7 +272,6 @@ void AC_PosControl::pos_to_rate_z() // calculates desired acceleration and calls accel throttle controller void AC_PosControl::rate_to_accel_z(float vel_target_z) { - uint32_t now = hal.scheduler->millis(); const Vector3f& curr_vel = _inav.get_velocity(); float z_target_speed_delta; // The change in requested speed float p; // used to capture pid values for logging @@ -277,11 +291,12 @@ void AC_PosControl::rate_to_accel_z(float vel_target_z) } // reset velocity error and filter if this controller has just been engaged - if (now - _last_update_rate_ms > 100 ) { + if (_flags.reset_rate_to_accel_z) { // Reset Filter _vel_error.z = 0; _vel_target_filt_z = vel_target_z; desired_accel = 0; + _flags.reset_rate_to_accel_z = false; } else { // calculate rate error and filter with cut off frequency of 2 Hz //To-Do: adjust constant below based on update rate @@ -291,7 +306,6 @@ void AC_PosControl::rate_to_accel_z(float vel_target_z) _vel_target_filt_z = _vel_target_filt_z + z_target_speed_delta; desired_accel = z_target_speed_delta / _dt; } - _last_update_rate_ms = now; // calculate p p = _p_alt_rate.kP() * _vel_error.z; @@ -311,7 +325,6 @@ void AC_PosControl::rate_to_accel_z(float vel_target_z) // calculates a desired throttle which is sent directly to the motors void AC_PosControl::accel_to_throttle(float accel_target_z) { - uint32_t now = hal.scheduler->millis(); float z_accel_meas; // actual acceleration int32_t p,i,d; // used to capture pid values for logging @@ -319,15 +332,15 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) z_accel_meas = -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f; // reset target altitude if this controller has just been engaged - if (now - _last_update_accel_ms > 100) { + if (_flags.reset_accel_to_throttle) { // Reset Filter _accel_error.z = 0; + _flags.reset_accel_to_throttle = false; } else { // calculate accel error and Filter with fc = 2 Hz // To-Do: replace constant below with one that is adjusted for update rate _accel_error.z = _accel_error.z + 0.11164f * (constrain_float(accel_target_z - z_accel_meas, -32000, 32000) - _accel_error.z); } - _last_update_accel_ms = now; // separately calculate p, i, d values for logging p = _pid_alt_accel.get_p(_accel_error.z); @@ -449,8 +462,8 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity) { // catch if we've just been started uint32_t now = hal.scheduler->millis(); - if ((now - _last_update_ms) >= 1000) { - _last_update_ms = now; + if ((now - _last_update_xy_ms) >= POSCONTROL_ACTIVE_TIMEOUT_MS) { + _last_update_xy_ms = now; if (!_flags.keep_xy_I_terms) { reset_I_xy(); } @@ -472,8 +485,8 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity) switch (_xy_step) { case 0: // capture time since last iteration - _dt_xy = (now - _last_update_ms) / 1000.0f; - _last_update_ms = now; + _dt_xy = (now - _last_update_xy_ms) / 1000.0f; + _last_update_xy_ms = now; // translate any adjustments from pilot to loiter target desired_vel_to_pos(_dt_xy); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index cf76337900..75095a9c9e 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -34,6 +34,8 @@ #define POSCONTROL_DT_10HZ 0.10f // time difference in seconds for 10hz update rate +#define POSCONTROL_ACTIVE_TIMEOUT_MS 200 // position controller is considered active if it has been called within the past 200ms (0.2 seconds) + class AC_PosControl { public: @@ -117,6 +119,9 @@ public: /// init_takeoff - initialises target altitude if we are taking off void init_takeoff(); + // is_active - returns true if the z-axis position controller has been run very recently + bool is_active_z() const; + /// update_z_controller - fly to altitude in cm above home void update_z_controller(); @@ -207,6 +212,8 @@ private: uint8_t force_recalc_xy : 1; // 1 if we want the xy position controller to run at it's next possible time. set by loiter and wp controllers after they have updated the target position. uint8_t slow_cpu : 1; // 1 if we are running on a slow_cpu machine. xy position control is broken up into multiple steps uint8_t keep_xy_I_terms : 1; // 1 if we are to keep I terms when the position controller is next run. Reset automatically back to zero in update_xy_controller. + uint8_t reset_rate_to_accel_z : 1; // 1 if we should reset the rate_to_accel_z step + uint8_t reset_accel_to_throttle : 1; // 1 if we should reset the accel_to_throttle step of the z-axis controller } _flags; // limit flags structure @@ -282,9 +289,8 @@ private: // internal variables float _dt; // time difference (in seconds) between calls from the main program - uint32_t _last_update_ms; // system time of last update_position_controller call - uint32_t _last_update_rate_ms; // system time of last call to rate_to_accel_z (alt hold accel controller) - uint32_t _last_update_accel_ms; // system time of last call to accel_to_throttle (alt hold accel controller) + uint32_t _last_update_xy_ms; // system time of last update_xy_controller call + uint32_t _last_update_z_ms; // system time of last update_z_controller call uint8_t _step; // used to decide which portion of position controller to run during this iteration float _speed_down_cms; // max descent rate in cm/s float _speed_up_cms; // max climb rate in cm/s