From de343598083d675e59f7e0275b6c5b74dc7cf11c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 24 Jan 2014 12:27:06 +0900 Subject: [PATCH] AC_PosControl: recalculate leash when speed or accel modified --- .../AC_AttitudeControl/AC_PosControl.cpp | 218 +++++++++++------- libraries/AC_AttitudeControl/AC_PosControl.h | 58 +++-- 2 files changed, 173 insertions(+), 103 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 86ab976b42..00639d26ab 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -66,16 +66,68 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, #else _flags.slow_cpu = true; #endif + _flags.recalc_leash_xy = true; + _flags.recalc_leash_z = true; } /// /// z-axis position controller /// +/// set_speed_z - sets maximum climb and descent rates +void AC_PosControl::set_speed_z(float speed_down, float speed_up) +{ + if ((fabs(_speed_down_cms-speed_down) > 1.0f) || (fabs(_speed_up_cms-speed_up) > 1.0f)) { + _speed_down_cms = speed_down; + _speed_up_cms = speed_up; + _flags.recalc_leash_z = true; + } +} + +/// set_accel_z - set vertical acceleration in cm/s/s +void AC_PosControl::set_accel_z(float accel_cmss) +{ + if (fabs(_accel_z_cms-accel_cmss) > 1.0f) { + _accel_z_cms = accel_cmss; + _flags.recalc_leash_z = true; + } +} + +/// set_alt_target_with_slew - adjusts target towards a final altitude target +/// 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::set_alt_target_with_slew(float alt_cm, float dt) +{ + float alt_change = alt_cm-_pos_target.z; + + // adjust desired alt if motors have not hit their limits + if ((alt_change<0 && !_motors.limit.throttle_lower) || (alt_change>0 && !_motors.limit.throttle_upper)) { + _pos_target.z += constrain_float(alt_change, _speed_down_cms*dt, _speed_up_cms*dt); + } + + // do not let target get too far from current altitude + float curr_alt = _inav.get_altitude(); + _pos_target.z = constrain_float(_pos_target.z,curr_alt-_leash_down_z,curr_alt+_leash_up_z); +} + +/// 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::set_alt_target_from_climb_rate(float climb_rate_cms, float dt) +{ + // adjust desired alt if motors have not hit their limits + // To-Do: add check of _limit.pos_up and _limit.pos_down? + if ((climb_rate_cms<0 && !_motors.limit.throttle_lower) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) { + _pos_target.z += climb_rate_cms * _dt; + } +} + // get_alt_error - returns altitude error in cm float AC_PosControl::get_alt_error() const { - return (_pos_target.z - _inav.get_position().z); + return (_pos_target.z - _inav.get_altitude()); } /// set_target_to_stopping_point_z - returns reasonable stopping altitude in cm above home @@ -119,43 +171,24 @@ void AC_PosControl::init_takeoff() /// update_z_controller - fly to altitude in cm above home void AC_PosControl::update_z_controller() { + // check if leash lengths need to be recalculated + calc_leash_length_z(); + // call position controller pos_to_rate_z(); } -/// climb_at_rate - climb at rate provided in cm/s -void AC_PosControl::climb_at_rate(const float rate_target_cms) +/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration +/// called by pos_to_rate_z if z-axis speed or accelerations are changed +void AC_PosControl::calc_leash_length_z() { - const Vector3f& curr_pos = _inav.get_position(); - - // clear position limit flags - _limit.pos_up = false; - _limit.pos_down = false; - - // adjust desired alt if motors have not hit their limits - // To-Do: should we use some other limits? this controller's vel limits? - if ((rate_target_cms<0 && !_motors.limit.throttle_lower) || (rate_target_cms>0 && !_motors.limit.throttle_upper)) { - _pos_target.z += rate_target_cms * _dt; + if (_flags.recalc_leash_z) { + _leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _pi_alt_pos.kP()); + _leash_down_z = calc_leash_length(_speed_down_cms, _accel_z_cms, _pi_alt_pos.kP()); + _flags.recalc_leash_z = false; + // debug -- remove me! + hal.console->printf_P(PSTR("\nLLZ:%4.2f %4.2f\n"),(float)_leash_up_z, (float)_leash_down_z); } - - // do not let target altitude get too far from current altitude - if (_pos_target.z < curr_pos.z - POSCONTROL_LEASH_Z) { - _pos_target.z = curr_pos.z - POSCONTROL_LEASH_Z; - _limit.pos_down = true; - } - if (_pos_target.z > curr_pos.z + POSCONTROL_LEASH_Z) { - _pos_target.z = curr_pos.z + POSCONTROL_LEASH_Z; - _limit.pos_up = true; - } - - // do not let target alt get above limit - if (_alt_max > 0 && _pos_target.z > _alt_max) { - _pos_target.z = _alt_max; - _limit.pos_up = true; - } - - // call position controller - pos_to_rate_z(); } // pos_to_rate_z - position to rate controller for Z axis @@ -163,11 +196,31 @@ void AC_PosControl::climb_at_rate(const float rate_target_cms) // vel_up_max, vel_down_max should have already been set before calling this method void AC_PosControl::pos_to_rate_z() { - const Vector3f& curr_pos = _inav.get_position(); + float curr_alt = _inav.get_altitude(); float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt. + // clear position limit flags + _limit.pos_up = false; + _limit.pos_down = false; + // calculate altitude error - _pos_error.z = _pos_target.z - curr_pos.z; + _pos_error.z = _pos_target.z - curr_alt; + + // do not let target altitude get too far from current altitude + if (_pos_error.z > _leash_up_z) { + _pos_target.z = curr_alt + _leash_up_z; + _limit.pos_up = true; + } + if (_pos_error.z < -_leash_down_z) { + _pos_target.z = curr_alt - _leash_down_z; + _limit.pos_down = true; + } + + // do not let target alt get above limit + if (_alt_max > 0 && _pos_target.z > _alt_max) { + _pos_target.z = _alt_max; + _limit.pos_up = true; + } // check kP to avoid division by zero if (_pi_alt_pos.kP() != 0) { @@ -285,68 +338,47 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) // to-do add back in PID logging? } -/* -// get_throttle_althold_with_slew - altitude controller with slew to avoid step changes in altitude target -// calls normal althold controller which updates accel based throttle controller targets -static void -get_throttle_althold_with_slew(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate) -{ - float alt_change = target_alt-controller_desired_alt; - // adjust desired alt if motors have not hit their limits - if ((alt_change<0 && !motors.limit.throttle_lower) || (alt_change>0 && !motors.limit.throttle_upper)) { - controller_desired_alt += constrain_float(alt_change, min_climb_rate*0.02f, max_climb_rate*0.02f); - } - - // do not let target altitude get too far from current altitude - controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750); - - get_throttle_althold(controller_desired_alt, min_climb_rate-250, max_climb_rate+250); // 250 is added to give head room to alt hold controller -} - -// get_throttle_rate_stabilized - rate controller with additional 'stabilizer' -// 'stabilizer' ensure desired rate is being met -// calls normal throttle rate controller which updates accel based throttle controller targets -static void -get_throttle_rate_stabilized(int16_t target_rate) -{ - // adjust desired alt if motors have not hit their limits - if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) { - controller_desired_alt += target_rate * 0.02f; - } - - // do not let target altitude get too far from current altitude - controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750); - -#if AC_FENCE == ENABLED - // do not let target altitude be too close to the fence - // To-Do: add this to other altitude controllers - if((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { - float alt_limit = fence.get_safe_alt() * 100.0f; - if (controller_desired_alt > alt_limit) { - controller_desired_alt = alt_limit; - } - } -#endif - - // update target altitude for reporting purposes - set_target_alt_for_reporting(controller_desired_alt); - - get_throttle_althold(controller_desired_alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller -} -*/ /// /// position controller /// +/// set_accel_xy - set horizontal acceleration in cm/s/s +/// calc_leash_length_xy should be called afterwards +void AC_PosControl::set_accel_xy(float accel_cmss) +{ + if (fabs(_accel_cms-accel_cmss) > 1.0f) { + _accel_cms = accel_cmss; + _flags.recalc_leash_xy = true; + } +} + +/// set_speed_xy - set horizontal speed maximum in cm/s +/// calc_leash_length_xy should be called afterwards +void AC_PosControl::set_speed_xy(float speed_cms) +{ + if (fabs(_speed_cms-speed_cms) > 1.0f) { + _speed_cms = speed_cms; + _flags.recalc_leash_xy = true; + } +} + /// set_pos_target in cm from home void AC_PosControl::set_pos_target(const Vector3f& position) { _pos_target = position; + // debug -- remove me! + static uint8_t counter = 0; + counter++; + if (counter >= 10) { + counter = 0; + hal.console->printf_P(PSTR("\nPosX:%4.2f Y:%4.2f Z:%4.2f\n"), (float)_pos_target.x, (float)_pos_target.y, (float)_pos_target.z); + } // initialise roll and pitch to current roll and pitch. This avoids a twitch between when the target is set and the pos controller is first run - _roll_target = constrain_int32(_ahrs.roll_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max()); - _pitch_target = constrain_int32(_ahrs.pitch_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max()); + // To-Do: this initialisation of roll and pitch targets needs to go somewhere between when pos-control is initialised and when it completes it's first cycle + //_roll_target = constrain_int32(_ahrs.roll_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max()); + //_pitch_target = constrain_int32(_ahrs.pitch_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max()); } /// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration @@ -408,6 +440,9 @@ void AC_PosControl::update_pos_controller(bool use_desired_velocity) _xy_step = 0; } + // check if xy leash needs to be recalculated + calc_leash_length_xy(); + // reset step back to 0 if loiter or waypoint parents have triggered an update and we completed the last full cycle if (_flags.force_recalc_xy && _xy_step > 3) { _flags.force_recalc_xy = false; @@ -447,6 +482,19 @@ void AC_PosControl::update_pos_controller(bool use_desired_velocity) /// private methods /// +/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration +/// should be called whenever the speed, acceleration or position kP is modified +void AC_PosControl::calc_leash_length_xy() +{ + if (_flags.recalc_leash_xy) { + _leash = calc_leash_length(_speed_cms, _accel_cms, _pi_pos_lon.kP()); + _flags.recalc_leash_xy = false; + // debug -- remove me! + hal.console->printf_P(PSTR("\nXYA:%4.2f S:%4.2f kP:%4.2f\n"),(float)_accel_cms, (float)_speed_cms, (float)_pi_pos_lon.kP()); + hal.console->printf_P(PSTR("\nLLXY:%4.2f\n"),(float)_leash); + } +} + /// desired_vel_to_pos - move position target using desired velocities void AC_PosControl::desired_vel_to_pos(float nav_dt) { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 32c91642a3..3cef902022 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -65,23 +65,39 @@ public: /// set_speed_z - sets maximum climb and descent rates /// To-Do: call this in the main code as part of flight mode initialisation /// calc_leash_length_z should be called afterwards - void set_speed_z(float speed_down, float speed_up) { _speed_down_cms = speed_down; _speed_up_cms = speed_up;} + void set_speed_z(float speed_down, float speed_up); /// set_accel_z - set vertical acceleration in cm/s/s /// calc_leash_length_z should be called afterwards - void set_accel_z(float accel_cmss) { _accel_z_cms = accel_cmss; } + void set_accel_z(float accel_cmss); - // set_throttle_hover - update estimated throttle required to maintain hover + /// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration + /// called by pos_to_rate_z if z-axis speed or accelerations are changed + void calc_leash_length_z(); + + /// set_throttle_hover - update estimated throttle required to maintain hover void set_throttle_hover(float throttle) { _throttle_hover = throttle; } - // set_alt_target - set altitude target in cm above home + /// set_alt_target - set altitude target in cm above home void set_alt_target(float alt_cm) { _pos_target.z = alt_cm; } + /// set_alt_target_with_slew - adjusts target towards a final altitude target + /// 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 set_alt_target_with_slew(float alt_cm, float dt); + + /// 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 set_alt_target_from_climb_rate(float climb_rate_cms, float dt); + /// get_alt_target, get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller /// To-Do: remove one of the two functions below float get_alt_target() const { return _pos_target.z; } - // get_alt_error - returns altitude error in cm + /// get_alt_error - returns altitude error in cm float get_alt_error() const; /// set_target_to_stopping_point_z - sets altitude target to reasonable stopping altitude in cm above home @@ -93,35 +109,28 @@ public: /// update_z_controller - fly to altitude in cm above home void update_z_controller(); - /// climb_at_rate - climb at rate provided in cm/s - void climb_at_rate(const float rate_target_cms); + // get_leash_down_z, get_leash_up_z - returns vertical leash lengths in cm + float get_leash_down_z() { return _leash_down_z; } + float get_leash_up_z() { return _leash_up_z; } /// althold_kP - returns altitude hold position control PID's kP gain float althold_kP() { return _pi_alt_pos.kP(); } - /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration - /// should be called whenever the speed, acceleration or position kP is modified - float calc_leash_length_z(float speed_cms, float accel_cms) const { return calc_leash_length(speed_cms, accel_cms, _pi_alt_pos.kP()); } - /// /// xy position controller /// /// set_accel_xy - set horizontal acceleration in cm/s/s /// calc_leash_length_xy should be called afterwards - void set_accel_xy(float accel_cmss) { _accel_cms = accel_cmss; } + void set_accel_xy(float accel_cmss); /// set_speed_xy - set horizontal speed maximum in cm/s /// calc_leash_length_xy should be called afterwards - void set_speed_xy(float speed_cms) { _speed_cms = speed_cms; } - - /// set_leash_xy - sets horizontal leash lenght - /// should be based on results from calc_leash_length_xy() method - void set_leash_xy(float leash_cm) { _leash = leash_cm; } + void set_speed_xy(float speed_cms); /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration /// should be called whenever the speed, acceleration or position kP is modified - float calc_leash_length_xy(float speed_cms, float accel_cms) const { return calc_leash_length(speed_cms, accel_cms, _pi_pos_lon.kP()); } + void calc_leash_length_xy(); /// get_pos_target - get target as position vector (from home in cm) const Vector3f& get_pos_target() const { return _pos_target; } @@ -158,6 +167,9 @@ public: float get_roll() const { return _roll_target; } float get_pitch() const { return _pitch_target; } + // get_leash_xy - returns horizontal leash length in cm + float get_leash_xy() { return _leash; } + /// accessors for reporting const Vector3f get_vel_target() { return _vel_target; } const Vector3f get_accel_target() { return _accel_target; } @@ -190,6 +202,10 @@ private: uint8_t accel_xy : 1; // 1 if we have hit the horizontal accel limit } _limit; + /// + /// z controller private methods + /// + // pos_to_rate_z - position to rate controller for Z axis // target altitude should be placed into _pos_target.z using or set with one of these functions // set_alt_target @@ -203,6 +219,10 @@ private: // accel_to_throttle - alt hold's acceleration controller void accel_to_throttle(float accel_target_z); + /// + /// xy controller private methods + /// + /// desired_vel_to_pos - move position target using desired velocities void desired_vel_to_pos(float nav_dt); @@ -257,6 +277,8 @@ private: float _accel_z_cms; // max vertical acceleration in cm/s/s float _accel_cms; // max horizontal acceleration in cm/s/s float _leash; // horizontal leash length in cm. target will never be further than this distance from the vehicle + float _leash_down_z; // vertical leash down in cm. target will never be further than this distance below the vehicle + float _leash_up_z; // vertical leash up in cm. target will never be further than this distance above the vehicle float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame float _sin_yaw; float _cos_pitch;