Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: add init take-off

This commit is contained in:
Randy Mackay 2013-12-30 22:12:59 +09:00 committed by Andrew Tridgell
parent 2c03a3a3c6
commit 8988b48ad8
2 changed files with 43 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -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) {

25
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -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);