From 8988b48ad8aca8e7b7b5c0c9b8724446081f90f0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 30 Dec 2013 22:12:59 +0900 Subject: [PATCH] AC_PosControl: add init take-off --- .../AC_AttitudeControl/AC_PosControl.cpp | 38 ++++++++++++------- libraries/AC_AttitudeControl/AC_PosControl.h | 25 ++++++++---- 2 files changed, 43 insertions(+), 20 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 2ec9e7d81c..64b4462fad 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -61,12 +61,11 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, /// z-axis position controller /// -/// get_stopping_point_z - returns reasonable stopping altitude in cm above home -float AC_PosControl::get_stopping_point_z() +/// set_target_to_stopping_point_z - returns reasonable stopping altitude in cm above home +void AC_PosControl::set_target_to_stopping_point_z() { const Vector3f& curr_pos = _inav.get_position(); const Vector3f& curr_vel = _inav.get_velocity(); - float target_alt; float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt float linear_velocity; // the velocity we swap between linear and sqrt @@ -75,23 +74,36 @@ float AC_PosControl::get_stopping_point_z() if (fabs(curr_vel.z) < linear_velocity) { // if our current velocity is below the cross-over point we use a linear function - target_alt = curr_pos.z + curr_vel.z/_pi_alt_pos.kP(); + _pos_target.z = curr_pos.z + curr_vel.z/_pi_alt_pos.kP(); } else { linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/(2.0f*_pi_alt_pos.kP()*_pi_alt_pos.kP()); if (curr_vel.z > 0){ - target_alt = curr_pos.z + (linear_distance + curr_vel.z*curr_vel.z/(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX)); + _pos_target.z = curr_pos.z + (linear_distance + curr_vel.z*curr_vel.z/(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX)); } else { - target_alt = curr_pos.z - (linear_distance + curr_vel.z*curr_vel.z/(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX)); + _pos_target.z = curr_pos.z - (linear_distance + curr_vel.z*curr_vel.z/(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX)); } } - return constrain_float(target_alt, curr_pos.z - POSCONTROL_STOPPING_DIST_Z_MAX, curr_pos.z + POSCONTROL_STOPPING_DIST_Z_MAX); + _pos_target.z = constrain_float(_pos_target.z, curr_pos.z - POSCONTROL_STOPPING_DIST_Z_MAX, curr_pos.z + POSCONTROL_STOPPING_DIST_Z_MAX); } -/// fly_to_z - fly to altitude in cm above home -void AC_PosControl::fly_to_z(const float alt_cm) +/// init_takeoff - initialises target altitude if we are taking off +void AC_PosControl::init_takeoff() +{ + const Vector3f& curr_pos = _inav.get_position(); + + _pos_target.z = curr_pos.z + POSCONTROL_TAKEOFF_JUMP_CM; + + // clear i term from acceleration controller + if (_pid_alt_accel.get_integrator() < 0) { + _pid_alt_accel.reset_I(); + } +} + +/// fly_to_target_z - fly to altitude in cm above home +void AC_PosControl::fly_to_target_z() { // call position controller - pos_to_rate_z(alt_cm); + pos_to_rate_z(); } /// climb_at_rate - climb at rate provided in cm/s @@ -126,19 +138,19 @@ void AC_PosControl::climb_at_rate(const float rate_target_cms) } // call position controller - pos_to_rate_z(_pos_target.z); + pos_to_rate_z(); } // pos_to_rate_z - position to rate controller for Z axis // calculates desired rate in earth-frame z axis and passes to rate controller // vel_up_max, vel_down_max should have already been set before calling this method -void AC_PosControl::pos_to_rate_z(float alt_cm) +void AC_PosControl::pos_to_rate_z() { const Vector3f& curr_pos = _inav.get_position(); float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt. // calculate altitude error - _pos_error.z = alt_cm - curr_pos.z; + _pos_error.z = _pos_target.z - curr_pos.z; // check kP to avoid division by zero if (_pi_alt_pos.kP() != 0) { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 136daf18fc..d97f701a00 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -22,6 +22,7 @@ #define POSCONTROL_STOPPING_DIST_Z_MAX 200.0f // max stopping distance vertically // should be 1.5 times larger than POSCONTROL_ACCELERATION. // max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s +#define POSCONTROL_TAKEOFF_JUMP_CM 20.0f // during take-off altitude target is set to current altitude + this value #define POSCONTROL_SPEED 500.0f // maximum default loiter speed in cm/s #define POSCONTROL_VEL_Z_MIN -150.0f // default minimum climb velocity (i.e. max descent rate). To-Do: subtract 250 from this? @@ -50,7 +51,7 @@ public: /// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025) void set_dt(float delta_sec) { _dt = delta_sec; } - float get_dt() { return _dt; } + float get_dt() const { return _dt; } /// set_alt_max - sets maximum altitude above home in cm /// set to zero to disable limit @@ -65,11 +66,19 @@ public: /// z position controller /// - /// get_stopping_point_z - returns reasonable stopping altitude in cm above home - float get_stopping_point_z(); + void set_alt_target(float alt_cm) { _pos_target.z = alt_cm; } + float get_alt_target() const { return _pos_target.z; } + /// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller + float get_desired_alt() const { return _pos_target.z; } + + /// set_target_to_stopping_point_z - sets altitude target to reasonable stopping altitude in cm above home + void set_target_to_stopping_point_z(); + + /// init_takeoff - initialises target altitude if we are taking off + void init_takeoff(); /// fly_to_z - fly to altitude in cm above home - void fly_to_z(const float alt_cm); + void fly_to_target_z(); /// climb_at_rate - climb at rate provided in cm/s void climb_at_rate(const float rate_target_cms); @@ -108,8 +117,6 @@ public: int32_t get_desired_roll() const { return _desired_roll; }; int32_t get_desired_pitch() const { return _desired_pitch; }; */ - /// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller - float get_desired_alt() const { return _pos_target.z; } /// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame void set_cos_sin_yaw(float cos_yaw, float sin_yaw, float cos_pitch) { @@ -153,7 +160,11 @@ private: } _limit; // pos_to_rate_z - position to rate controller for Z axis - void pos_to_rate_z(float alt_cm); + // target altitude should be placed into _pos_target.z using or set with one of these functions + // set_alt_target + // set_target_to_stopping_point_z + // init_takeoff + void pos_to_rate_z(); // rate_to_accel_z - calculates desired accel required to achieve the velocity target void rate_to_accel_z(float vel_target_z);