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

AC_PosControl: add throttle controller

This commit is contained in:
Randy Mackay 2013-12-28 23:04:45 +09:00 committed by Andrew Tridgell
parent 05bb943a69
commit dcac124105
2 changed files with 295 additions and 614 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

782
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -5,34 +5,13 @@
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AC_PosControl::var_info[] PROGMEM = {
// index 0 was used for the old orientation matrix
// @Param: SPEED
// @DisplayName: Position Controller Maximum Horizontal Speed
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally during a WP mission
// @Units: cm/s
// @Range: 0 2000
// @Increment: 50
// @User: Standard
AP_GROUPINFO("SPEED", 0, AC_PosControl, _wp_speed_cms, WPNAV_WP_SPEED),
// @Param: SPEED_UP
// @DisplayName: Position Controller Maximum Speed Up
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while climbing during a WP mission
// @Units: cm/s
// @Param: THR_HOVER
// @DisplayName: Throttle Hover
// @Description: The autopilot's estimate of the throttle required to maintain a level hover. Calculated automatically from the pilot's throttle input while in stabilize mode
// @Range: 0 1000
// @Increment: 50
// @User: Standard
AP_GROUPINFO("SPEED_UP", 1, AC_PosControl, _wp_speed_up_cms, WPNAV_WP_SPEED_UP),
// @Param: SPEED_DN
// @DisplayName: Position Controller Maximum Speed Down
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while descending during a WP mission
// @Units: cm/s
// @Range: 0 1000
// @Increment: 50
// @User: Standard
AP_GROUPINFO("SPEED_DN", 2, AC_PosControl, _wp_speed_down_cms, WPNAV_WP_SPEED_DOWN),
// @Units: Percent*10
// @User: Advanced
AP_GROUPINFO("THR_HOVER", 0, AC_PosControl, _throttle_hover, POSCONTROL_THROTTLE_HOVER),
AP_GROUPEND
};
@ -41,101 +20,243 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] PROGMEM = {
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_PosControl::AC_PosControl(const AP_InertialNav& inav, const AP_AHRS& ahrs, const AC_AttitudeControl& attitude_control,
AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control,
APM_PI& pi_alt_pos, AC_PID& pid_alt_rate, AC_PID& pid_alt_accel,
APM_PI& pi_pos_lat, APM_PI& pi_pos_lon, AC_PID& pid_rate_lat, AC_PID& pid_rate_lon)
_inav(inav),
APM_PI& pi_pos_lat, APM_PI& pi_pos_lon, AC_PID& pid_rate_lat, AC_PID& pid_rate_lon) :
_ahrs(ahrs),
_inav(inav),
_motors(motors),
_attitude_control(attitude_control),
_pi_alt_hold(pi_alt_pos),
_pi_alt_pos(pi_alt_pos),
_pid_alt_rate(pid_alt_rate),
_pid_alt_accel(pid_alt_accel),
_pid_pos_lat(pid_pos_lat),
_pid_pos_lon(pid_pos_lon),
_pi_pos_lat(pi_pos_lat),
_pi_pos_lon(pi_pos_lon),
_pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon),
_last_update(0),
_dt(POSCONTROL_DT_10HZ),
_last_update_ms(0),
_last_update_rate_ms(0),
_last_update_accel_ms(0),
_step(0),
_speed_cms(POSCONTROL_SPEED),
_vel_z_min(POSCONTROL_VEL_Z_MIN),
_vel_z_max(POSCONTROL_VEL_Z_MAX),
_accel_cms(POSCONTROL_ACCEL_XY_MAX), // To-Do: check this default
_cos_yaw(1.0),
_sin_yaw(0.0),
_cos_pitch(1.0),
_desired_roll(0),
_desired_pitch(0),
_target(0,0,0),
_target_vel(0,0,0),
_vel_last(0,0,0),
_loiter_leash(WPNAV_MIN_LEASH_LENGTH),
_loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX),
_lean_angle_max_cd(MAX_LEAN_ANGLE),
desired_vel(0,0),
desired_accel(0,0)
_desired_roll(0.0),
_desired_pitch(0.0),
_leash(POSCONTROL_LEASH_LENGTH_MIN)
{
AP_Param::setup_object_defaults(this, var_info);
// calculate loiter leash
calculate_leash_length();
// calculate leash length
//calculate_leash_length();
}
// get_initial_alt_hold - get new target altitude based on current altitude and climb rate
static int32_t
get_initial_alt_hold( int32_t alt_cm, int16_t climb_rate_cms)
///
/// z-axis position controller
///
/// get_stopping_point_z - returns reasonable stopping altitude in cm above home
float AC_PosControl::get_stopping_point_z()
{
int32_t target_alt;
int32_t linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt.
int32_t linear_velocity; // the velocity we swap between linear and sqrt.
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
linear_velocity = ALT_HOLD_ACCEL_MAX/g.pi_alt_hold.kP();
// calculate the velocity at which we switch from calculating the stopping point using a linear funcction to a sqrt function
linear_velocity = POSCONTROL_ALT_HOLD_ACCEL_MAX/_pi_alt_pos.kP();
if (abs(climb_rate_cms) < linear_velocity) {
target_alt = alt_cm + climb_rate_cms/g.pi_alt_hold.kP();
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();
} else {
linear_distance = ALT_HOLD_ACCEL_MAX/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if (climb_rate_cms > 0){
target_alt = alt_cm + linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX);
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));
} else {
target_alt = alt_cm - ( linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX) );
target_alt = curr_pos.z - (linear_distance + curr_vel.z*curr_vel.z/(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX));
}
}
return constrain_int32(target_alt, alt_cm - ALT_HOLD_INIT_MAX_OVERSHOOT, alt_cm + ALT_HOLD_INIT_MAX_OVERSHOOT);
return constrain_float(target_alt, curr_pos.z - POSCONTROL_STOPPING_DIST_Z_MAX, curr_pos.z + POSCONTROL_STOPPING_DIST_Z_MAX);
}
// get_throttle_althold - hold at the desired altitude in cm
// updates accel based throttle controller targets
// Note: max_climb_rate is an optional parameter to allow reuse of this function by landing controller
static void
get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate)
/// fly_to_z - fly to altitude in cm above home
void AC_PosControl::fly_to_z(const float alt_cm)
{
int32_t alt_error;
float desired_rate;
int32_t linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt.
// call position controller
pos_to_rate_z(alt_cm);
}
/// climb_at_rate - climb at rate provided in cm/s
void AC_PosControl::climb_at_rate(const float rate_target_cms)
{
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;
}
// 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_target.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)
{
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
alt_error = target_alt - current_loc.alt;
_pos_error.z = alt_cm - curr_pos.z;
// check kP to avoid division by zero
if( g.pi_alt_hold.kP() != 0 ) {
linear_distance = ALT_HOLD_ACCEL_MAX/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if( alt_error > 2*linear_distance ) {
desired_rate = safe_sqrt(2*ALT_HOLD_ACCEL_MAX*(alt_error-linear_distance));
}else if( alt_error < -2*linear_distance ) {
desired_rate = -safe_sqrt(2*ALT_HOLD_ACCEL_MAX*(-alt_error-linear_distance));
if (_pi_alt_pos.kP() != 0) {
linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/(2.0f*_pi_alt_pos.kP()*_pi_alt_pos.kP());
if (_pos_error.z > 2*linear_distance ) {
_vel_target.z = safe_sqrt(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX*(_pos_error.z-linear_distance));
}else if (_pos_error.z < -2.0f*linear_distance) {
_vel_target.z = -safe_sqrt(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX*(-_pos_error.z-linear_distance));
}else{
desired_rate = g.pi_alt_hold.get_p(alt_error);
_vel_target.z = _pi_alt_pos.get_p(_pos_error.z);
}
}else{
desired_rate = 0;
_vel_target.z = 0;
}
desired_rate = constrain_float(desired_rate, min_climb_rate, max_climb_rate);
// call rate based throttle controller which will update accel based throttle controller targets
get_throttle_rate(desired_rate);
// update altitude error reported to GCS
altitude_error = alt_error;
// TO-DO: enabled PID logging for this controller
rate_to_accel_z(_vel_target.z);
}
// rate_to_accel_z - calculates desired accel required to achieve the velocity target
// 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
float desired_accel; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors
// check speed limits
// To-Do: check these speed limits here or in the pos->rate controller
_limit.vel_up = false;
_limit.vel_down = false;
if (_vel_target.z < _vel_z_min) {
_vel_target.z = _vel_z_min;
_limit.vel_down = true;
}
if (_vel_target.z > _vel_z_max) {
_vel_target.z = _vel_z_max;
_limit.vel_up = true;
}
// reset velocity error and filter if this controller has just been engaged
if (now - _last_update_rate_ms > 100 ) {
// Reset Filter
_vel_error.z = 0;
_vel_target_filt_z = vel_target_z;
desired_accel = 0;
} else {
// calculate rate error and filter with cut off frequency of 2 Hz
//To-Do: adjust constant below based on update rate
_vel_error.z = _vel_error.z + 0.20085f * ((vel_target_z - curr_vel.z) - _vel_error.z);
// feed forward acceleration based on change in the filtered desired speed.
z_target_speed_delta = 0.20085f * (vel_target_z - _vel_target_filt_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 = _pid_alt_rate.kP() * _vel_error.z;
// consolidate and constrain target acceleration
desired_accel += p;
desired_accel = constrain_int32(desired_accel, -32000, 32000);
// To-Do: re-enable PID logging?
// TO-DO: ensure throttle cruise is updated some other way in the main code or attitude control
// set target for accel based throttle controller
accel_to_throttle(desired_accel);
}
// accel_to_throttle - alt hold's acceleration controller
// 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
// Calculate Earth Frame Z acceleration
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) {
// Reset Filter
_accel_error.z = 0;
} 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);
// get i term
i = _pid_alt_accel.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
// To-Do: should this be replaced with limits check from attitude_controller?
if ((!_motors.limit.throttle_lower && !_motors.limit.throttle_upper) || (i>0&&_accel_error.z<0) || (i<0&&_accel_error.z>0)) {
i = _pid_alt_accel.get_i(_accel_error.z, _dt);
}
// get d term
d = _pid_alt_accel.get_d(_accel_error.z, _dt);
// To-Do: pull min/max throttle from motors
// To-Do: where to get hover throttle?
// To-Do: we had a contraint here but it's now removed, is this ok? with the motors library handle it ok?
_attitude_control.set_throttle_out((int16_t)p+i+d+_throttle_hover, true);
// 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
@ -183,119 +304,12 @@ get_throttle_rate_stabilized(int16_t target_rate)
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
}
// get_throttle_rate - calculates desired accel required to achieve desired z_target_speed
// sets accel based throttle controller target
static void
get_throttle_rate(float z_target_speed)
{
static uint32_t last_call_ms = 0;
static float z_rate_error = 0; // The velocity error in cm.
static float z_target_speed_filt = 0; // The filtered requested speed
float z_target_speed_delta; // The change in requested speed
int32_t p; // used to capture pid values for logging
int32_t output; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors
uint32_t now = millis();
// reset target altitude if this controller has just been engaged
if( now - last_call_ms > 100 ) {
// Reset Filter
z_rate_error = 0;
z_target_speed_filt = z_target_speed;
output = 0;
} else {
// calculate rate error and filter with cut off frequency of 2 Hz
z_rate_error = z_rate_error + 0.20085f * ((z_target_speed - climb_rate) - z_rate_error);
// feed forward acceleration based on change in the filtered desired speed.
z_target_speed_delta = 0.20085f * (z_target_speed - z_target_speed_filt);
z_target_speed_filt = z_target_speed_filt + z_target_speed_delta;
output = z_target_speed_delta * 50.0f; // To-Do: replace 50 with dt
}
last_call_ms = now;
// calculate p
p = g.pid_throttle_rate.kP() * z_rate_error;
// consolidate and constrain target acceleration
output += p;
output = constrain_int32(output, -32000, 32000);
#if LOGGING_ENABLED == ENABLED
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THROTTLE_RATE_KP || g.radio_tuning == CH6_THROTTLE_RATE_KD) ) {
pid_log_counter++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz
pid_log_counter = 0;
Log_Write_PID(CH6_THROTTLE_RATE_KP, z_rate_error, p, 0, 0, output, tuning_value);
}
}
#endif
// set target for accel based throttle controller
set_throttle_accel_target(output);
// update throttle cruise
// TO-DO: this may not be correct because g.rc_3.servo_out has not been updated for this iteration
if( z_target_speed == 0 ) {
update_throttle_cruise(g.rc_3.servo_out);
}
}
///
/// throttle controller
///
// get_throttle_accel - accelerometer based throttle controller
// returns an actual throttle output (0 ~ 1000) to be sent to the motors
static int16_t
get_throttle_accel(int16_t z_target_accel)
{
static float z_accel_error = 0; // The acceleration error in cm.
static uint32_t last_call_ms = 0; // the last time this controller was called
int32_t p,i,d; // used to capture pid values for logging
int16_t output;
float z_accel_meas;
uint32_t now = millis();
// Calculate Earth Frame Z acceleration
z_accel_meas = -(ahrs.get_accel_ef().z + GRAVITY_MSS) * 100;
// reset target altitude if this controller has just been engaged
if( now - last_call_ms > 100 ) {
// Reset Filter
z_accel_error = 0;
} else {
// calculate accel error and Filter with fc = 2 Hz
z_accel_error = z_accel_error + 0.11164f * (constrain_float(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error);
}
last_call_ms = now;
// separately calculate p, i, d values for logging
p = g.pid_throttle_accel.get_p(z_accel_error);
// get i term
i = g.pid_throttle_accel.get_integrator();
// replace below with check of throttle limit from attitude_control
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if ((!motors.limit.throttle_lower && !motors.limit.throttle_upper) || (i>0&&z_accel_error<0) || (i<0&&z_accel_error>0)) {
i = g.pid_throttle_accel.get_i(z_accel_error, .01f);
}
d = g.pid_throttle_accel.get_d(z_accel_error, .01f);
output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);
// to-do add back in PID logging?
return output;
}
*/
///
/// position controller
///
/*
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void AC_PosControl::get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const
{
@ -331,16 +345,16 @@ void AC_PosControl::get_stopping_point(const Vector3f& position, const Vector3f&
target.z = position.z;
}
/// set_loiter_target in cm from home
void AC_PosControl::set_loiter_target(const Vector3f& position)
/// set_pos_target in cm from home
void AC_PosControl::set_pos_target(const Vector3f& position)
{
_target = position;
_target_vel.x = 0;
_target_vel.y = 0;
}
/// init_loiter_target - set initial loiter target based on current position and velocity
void AC_PosControl::init_loiter_target(const Vector3f& position, const Vector3f& velocity)
/// init_pos_target - set initial loiter target based on current position and velocity
void AC_PosControl::init_pos_target(const Vector3f& position, const Vector3f& velocity)
{
// set target position and velocity based on current pos and velocity
_target.x = position.x;
@ -361,73 +375,6 @@ void AC_PosControl::init_loiter_target(const Vector3f& position, const Vector3f&
_vel_last = _inav->get_velocity();
}
/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
void AC_PosControl::move_loiter_target(float control_roll, float control_pitch, float dt)
{
// convert pilot input to desired velocity in cm/s
_pilot_vel_forward_cms = -control_pitch * _loiter_accel_cms / 4500.0f;
_pilot_vel_right_cms = control_roll * _loiter_accel_cms / 4500.0f;
}
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void AC_PosControl::translate_loiter_target_movements(float nav_dt)
{
Vector2f target_vel_adj;
float vel_total;
// range check nav_dt
if( nav_dt < 0 ) {
return;
}
// check loiter speed and avoid divide by zero
if( _loiter_speed_cms < 100.0f) {
_loiter_speed_cms = 100.0f;
}
// rotate pilot input to lat/lon frame
target_vel_adj.x = (_pilot_vel_forward_cms*_cos_yaw - _pilot_vel_right_cms*_sin_yaw);
target_vel_adj.y = (_pilot_vel_forward_cms*_sin_yaw + _pilot_vel_right_cms*_cos_yaw);
// add desired change in velocity to current target velocit
_target_vel.x += target_vel_adj.x*nav_dt;
_target_vel.y += target_vel_adj.y*nav_dt;
if(_target_vel.x > 0 ) {
_target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms;
_target_vel.x = max(_target_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}else if(_target_vel.x < 0) {
_target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms;
_target_vel.x = min(_target_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}
if(_target_vel.y > 0 ) {
_target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms;
_target_vel.y = max(_target_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}else if(_target_vel.y < 0) {
_target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms;
_target_vel.y = min(_target_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}
// constrain the velocity vector and scale if necessary
vel_total = safe_sqrt(_target_vel.x*_target_vel.x + _target_vel.y*_target_vel.y);
if (vel_total > _loiter_speed_cms && vel_total > 0.0f) {
_target_vel.x = _loiter_speed_cms * _target_vel.x/vel_total;
_target_vel.y = _loiter_speed_cms * _target_vel.y/vel_total;
}
// update target position
_target.x += _target_vel.x * nav_dt;
_target.y += _target_vel.y * nav_dt;
// constrain target position to within reasonable distance of current location
Vector3f curr_pos = _inav->get_position();
Vector3f distance_err = _target - curr_pos;
float distance = safe_sqrt(distance_err.x*distance_err.x + distance_err.y*distance_err.y);
if (distance > _loiter_leash && distance > 0.0f) {
_target.x = curr_pos.x + _loiter_leash * distance_err.x/distance;
_target.y = curr_pos.y + _loiter_leash * distance_err.y/distance;
}
}
/// get_distance_to_target - get horizontal distance to loiter target in cm
float AC_PosControl::get_distance_to_target() const
{
@ -488,8 +435,8 @@ void AC_PosControl::update_loiter()
}
}
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void AC_PosControl::calculate_loiter_leash_length()
/// calculate_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void AC_PosControl::calculate_leash_length()
{
// get loiter position P
float kP = _pid_pos_lat->kP();
@ -523,231 +470,6 @@ void AC_PosControl::calculate_loiter_leash_length()
}
}
///
/// waypoint navigation
///
/// set_destination - set destination using cm from home
void AC_PosControl::set_destination(const Vector3f& destination)
{
// if waypoint controlls is active and copter has reached the previous waypoint use it for the origin
if( _flags.reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) {
_origin = _destination;
}else{
// otherwise calculate origin from the current position and velocity
get_stopping_point(_inav->get_position(), _inav->get_velocity(), _origin);
}
// set origin and destination
set_origin_and_destination(_origin, destination);
}
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
void AC_PosControl::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
{
// store origin and destination locations
_origin = origin;
_destination = destination;
Vector3f pos_delta = _destination - _origin;
// calculate leash lengths
bool climb = pos_delta.z >= 0; // climbing vs descending leads to different leash lengths because speed_up_cms and speed_down_cms can be different
_track_length = pos_delta.length(); // get track length
// calculate each axis' percentage of the total distance to the destination
if (_track_length == 0.0f) {
// avoid possible divide by zero
_pos_delta_unit.x = 0;
_pos_delta_unit.y = 0;
_pos_delta_unit.z = 0;
}else{
_pos_delta_unit = pos_delta/_track_length;
}
calculate_wp_leash_length(climb); // update leash lengths
// initialise intermediate point to the origin
_track_desired = 0;
_target = origin;
_flags.reached_destination = false;
// initialise the limited speed to current speed along the track
const Vector3f &curr_vel = _inav->get_velocity();
// get speed along track (note: we convert vertical speed into horizontal speed equivalent)
float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
_limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
// default waypoint back to slow
_flags.fast_waypoint = false;
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the wpnav controller is first run
_desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
// reset target velocity - only used by loiter controller's interpretation of pilot input
_target_vel.x = 0;
_target_vel.y = 0;
}
/// advance_target_along_track - move target location along track from origin to destination
void AC_PosControl::advance_target_along_track(float dt)
{
float track_covered;
Vector3f track_error;
float track_desired_max;
float track_desired_temp = _track_desired;
float track_extra_max;
// get current location
Vector3f curr_pos = _inav->get_position();
Vector3f curr_delta = curr_pos - _origin;
// calculate how far along the track we are
track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
Vector3f track_covered_pos = _pos_delta_unit * track_covered;
track_error = curr_delta - track_covered_pos;
// calculate the horizontal error
float track_error_xy = safe_sqrt(track_error.x*track_error.x + track_error.y*track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
track_extra_max = min(_track_leash_length*(_wp_leash_z-track_error_z)/_wp_leash_z, _track_leash_length*(_wp_leash_xy-track_error_xy)/_wp_leash_xy);
if(track_extra_max <0) {
track_desired_max = track_covered;
}else{
track_desired_max = track_covered + track_extra_max;
}
// get current velocity
const Vector3f &curr_vel = _inav->get_velocity();
// get speed along track
float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
// calculate point at which velocity switches from linear to sqrt
float linear_velocity = _wp_speed_cms;
float kP = _pid_pos_lat->kP();
if (kP >= 0.0f) { // avoid divide by zero
linear_velocity = _track_accel/kP;
}
// let the limited_speed_xy_cms be some range above or below current velocity along track
if (speed_along_track < -linear_velocity) {
// we are travelling fast in the opposite direction of travel to the waypoint so do not move the intermediate point
_limited_speed_xy_cms = 0;
}else{
// increase intermediate target point's velocity if not yet at target speed (we will limit it below)
if(dt > 0) {
if(track_desired_max > _track_desired) {
_limited_speed_xy_cms += 2.0 * _track_accel * dt;
}else{
// do nothing, velocity stays constant
_track_desired = track_desired_max;
}
}
// do not go over top speed
if(_limited_speed_xy_cms > _track_speed) {
_limited_speed_xy_cms = _track_speed;
}
// if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity
if (fabsf(speed_along_track) < linear_velocity) {
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity);
}
}
// advance the current target
track_desired_temp += _limited_speed_xy_cms * dt;
// do not let desired point go past the end of the segment
track_desired_temp = constrain_float(track_desired_temp, 0, _track_length);
_track_desired = max(_track_desired, track_desired_temp);
// recalculate the desired position
_target = _origin + _pos_delta_unit * _track_desired;
// check if we've reached the waypoint
if( !_flags.reached_destination ) {
if( _track_desired >= _track_length ) {
// "fast" waypoints are complete once the intermediate point reaches the destination
if (_flags.fast_waypoint) {
_flags.reached_destination = true;
}else{
// regular waypoints also require the copter to be within the waypoint radius
Vector3f dist_to_dest = curr_pos - _destination;
if( dist_to_dest.length() <= _wp_radius_cm ) {
_flags.reached_destination = true;
}
}
}
}
}
/// get_distance_to_destination - get horizontal distance to destination in cm
float AC_PosControl::get_distance_to_destination()
{
// get current location
Vector3f curr = _inav->get_position();
return pythagorous2(_destination.x-curr.x,_destination.y-curr.y);
}
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_PosControl::get_bearing_to_destination()
{
return get_bearing_cd(_inav->get_position(), _destination);
}
/// update_wpnav - run the wp controller - should be called at 10hz
void AC_PosControl::update_wpnav()
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _wpnav_last_update) / 1000.0f;
// catch if we've just been started
if( dt >= 1.0 ) {
dt = 0.0;
reset_I();
_wpnav_step = 0;
}
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt > 0.095f && _wpnav_step > 3) {
_wpnav_step = 0;
}
// run loiter steps
switch (_wpnav_step) {
case 0:
// capture time since last iteration
_wpnav_dt = dt;
_wpnav_last_update = now;
// advance the target if necessary
if (dt > 0.0f) {
advance_target_along_track(dt);
}
_wpnav_step++;
break;
case 1:
// run loiter's position to velocity step
get_loiter_position_to_velocity(_wpnav_dt, _wp_speed_cms);
_wpnav_step++;
break;
case 2:
// run loiter's velocity to acceleration step
get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, _wpnav_dt);
_wpnav_step++;
break;
case 3:
// run loiter's acceleration to lean angle step
get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y);
_wpnav_step++;
break;
}
}
///
/// shared methods
///
@ -880,78 +602,4 @@ void AC_PosControl::reset_I()
// set last velocity to current velocity
_vel_last = _inav->get_velocity();
}
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
void AC_PosControl::calculate_wp_leash_length(bool climb)
{
// get loiter position P
float kP = _pid_pos_lat->kP();
// sanity check acceleration and avoid divide by zero
if (_wp_accel_cms <= 0.0f) {
_wp_accel_cms = WPNAV_ACCELERATION_MIN;
}
// avoid divide by zero
if (kP <= 0.0f) {
_wp_leash_xy = WPNAV_MIN_LEASH_LENGTH;
return;
}
// calculate horiztonal leash length
if(_wp_speed_cms <= _wp_accel_cms / kP) {
// linear leash length based on speed close in
_wp_leash_xy = _wp_speed_cms / kP;
}else{
// leash length grows at sqrt of speed further out
_wp_leash_xy = (_wp_accel_cms / (2.0f*kP*kP)) + (_wp_speed_cms*_wp_speed_cms / (2.0f*_wp_accel_cms));
}
// ensure leash is at least 1m long
if( _wp_leash_xy < WPNAV_MIN_LEASH_LENGTH ) {
_wp_leash_xy = WPNAV_MIN_LEASH_LENGTH;
}
// calculate vertical leash length
float speed_vert;
if( climb ) {
speed_vert = _wp_speed_up_cms;
}else{
speed_vert = _wp_speed_down_cms;
}
if(speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / _althold_kP) {
// linear leash length based on speed close in
_wp_leash_z = speed_vert / _althold_kP;
}else{
// leash length grows at sqrt of speed further out
_wp_leash_z = (WPNAV_ALT_HOLD_ACCEL_MAX / (2.0*_althold_kP*_althold_kP)) + (speed_vert*speed_vert / (2*WPNAV_ALT_HOLD_ACCEL_MAX));
}
// ensure leash is at least 1m long
if( _wp_leash_z < WPNAV_MIN_LEASH_LENGTH ) {
_wp_leash_z = WPNAV_MIN_LEASH_LENGTH;
}
// length of the unit direction vector in the horizontal
float pos_delta_unit_xy = sqrt(_pos_delta_unit.x*_pos_delta_unit.x+_pos_delta_unit.y*_pos_delta_unit.y);
float pos_delta_unit_z = fabsf(_pos_delta_unit.z);
// calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel
if(pos_delta_unit_z == 0 && pos_delta_unit_xy == 0){
_track_accel = 0;
_track_speed = 0;
_track_leash_length = WPNAV_MIN_LEASH_LENGTH;
}else if(_pos_delta_unit.z == 0){
_track_accel = _wp_accel_cms/pos_delta_unit_xy;
_track_speed = _wp_speed_cms/pos_delta_unit_xy;
_track_leash_length = _wp_leash_xy/pos_delta_unit_xy;
}else if(pos_delta_unit_xy == 0){
_track_accel = WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z;
_track_speed = speed_vert/pos_delta_unit_z;
_track_leash_length = _wp_leash_z/pos_delta_unit_z;
}else{
_track_accel = min(WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy);
_track_speed = min(speed_vert/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);
_track_leash_length = min(_wp_leash_z/pos_delta_unit_z, _wp_leash_xy/pos_delta_unit_xy);
}
}
*/

119
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -9,27 +9,28 @@
#include <AC_PID.h> // PID library
#include <APM_PI.h> // PID library
#include <AP_InertialNav.h> // Inertial Navigation library
#include <AC_AttitudeControl.h> // Attitude control library
#include <AP_Motors.h> // motors library
// loiter maximum velocities and accelerations
// position controller default definitions
#define POSCONTROL_THROTTLE_HOVER 450.0f // default throttle required to maintain hover
#define POSCONTROL_LEASH_Z 750.0f // leash length for z-axis altitude controller. To-Do: replace this with calculation based on alt_pos.kP()?
#define POSCONTROL_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller
#define POSCONTROL_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter
#define POSCONTROL_ACCEL_MAX 980.0f // max acceleration in cm/s/s that the loiter velocity controller will ask from the lower accel controller.
#define POSCONTROL_ACCEL_XY_MAX 980.0f // max horizontal acceleration in cm/s/s that the position velocity controller will ask from the lower accel controller
#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_SPEED 500.0f // maximum default loiter speed in cm/s
#define POSCONTROL_SPEED_UP 250.0f // default maximum climb velocity
#define POSCONTROL_SPEED_DOWN 150.0f // default maximum descent velocity
#define POSCONTROL_ACCEL_MAX 250.0f // maximum acceleration in loiter mode
#define POSCONTROL_ACCEL_MIN 25.0f // minimum acceleration in loiter mode
#define POSCONTROL_VEL_Z_MIN -150.0f // default minimum climb velocity (i.e. max descent rate). To-Do: subtract 250 from this?
#define POSCONTROL_VEL_Z_MAX 250.0f // default maximum climb velocity. To-Do: add 250 to this?
#define POSCONTROL_SPEED_MAX_TO_CORRECT_ERROR 200.0f // maximum speed used to correct position error (i.e. not including feed forward)
#define POSCONTROL_LEAN_ANGLE_MAX 4500 // default maximum lean angle
#define POSCONTROL_ALT_HOLD_P 1.0f // default throttle controller's altitude hold's P gain.
#define POSCONTROL_ALT_HOLD_ACCEL_MAX 250.0f // hard coded copy of throttle controller's maximum acceleration in cm/s. To-Do: remove duplication with throttle controller definition
#define POSCONTROL_MIN_LEASH_LENGTH 100.0f // minimum leash lengths in cm
#define POSCONTROL_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm
#define POSCONTROL_DT_10HZ 0.10f // time difference in seconds for 10hz update rate
@ -38,7 +39,8 @@ class AC_PosControl
public:
/// Constructor
AC_PosControl(const AP_InertialNav& inav, const AP_AHRS& ahrs, const AC_AttitudeControl& attitude_control,
AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control,
APM_PI& pi_alt_pos, AC_PID& pid_alt_rate, AC_PID& pid_alt_accel,
APM_PI& pi_pos_lat, APM_PI& pi_pos_lon, AC_PID& pid_rate_lat, AC_PID& pid_rate_lon);
@ -50,6 +52,15 @@ public:
void set_dt(float delta_sec) { _dt = delta_sec; }
float get_dt() { return _dt; }
/// set_alt_max - sets maximum altitude above home in cm
/// set to zero to disable limit
/// To-Do: update this intermittantly from main code after checking if fence is enabled/disabled
void set_alt_max(float alt) { _alt_max = alt; }
/// set_vel_z_limits - sets maximum climb and descent rates
/// To-Do: call this in the main code as part of flight mode initialisation
void set_vel_z_limits(float vel_min, float vel_max) { _vel_z_min = vel_min; _vel_z_max = vel_max;}
///
/// z position controller
///
@ -60,9 +71,10 @@ public:
/// fly_to_z - fly to altitude in cm above home
void fly_to_z(const float alt_cm);
/// climb - climb at rate provided in cm/s
void climb(const float rate_cms);
/// climb_at_rate - climb at rate provided in cm/s
void climb_at_rate(const float rate_target_cms);
/*
///
/// xy position controller
///
@ -95,12 +107,9 @@ public:
/// get desired roll, pitch which should be fed into stabilize controllers
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 _target.z; }
/// set_desired_alt - set desired altitude (in cm above home)
void set_desired_alt(float desired_alt) { _target.z = desired_alt; }
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) {
@ -109,9 +118,10 @@ public:
_cos_pitch = cos_pitch;
}
/// set_althold_kP - pass in alt hold controller's P gain
void set_althold_kP(float kP) { if(kP>0.0) _althold_kP = kP; }
// set_throttle_hover - update estimated throttle required to maintain hover
void set_throttle_hover(float throttle) { _throttle_hover = throttle; }
/*
/// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation
void set_horizontal_velocity(float velocity_cms) { _wp_speed_cms = velocity_cms; };
@ -129,18 +139,29 @@ public:
/// get_waypoint_acceleration - returns acceleration in cm/s/s during missions
float get_waypoint_acceleration() const { return _wp_accel_cms.get(); }
/// set_lean_angle_max - limits maximum lean angle
void set_lean_angle_max(int16_t angle_cd) { if (angle_cd >= 1000 && angle_cd <= 8000) {_lean_angle_max_cd = angle_cd;} }
*/
static const struct AP_Param::GroupInfo var_info[];
protected:
private:
// flags structure
struct poscontroller_flags {
uint8_t dummy : 1; // dummy flag
} _flags;
uint8_t pos_up : 1; // 1 if we have hit the vertical position leash limit while going up
uint8_t pos_down : 1; // 1 if we have hit the vertical position leash limit while going down
uint8_t vel_up : 1; // 1 if we have hit the vertical velocity limit going up
uint8_t vel_down : 1; // 1 if we have hit the vertical velocity limit going down
} _limit;
// pos_to_rate_z - position to rate controller for Z axis
void pos_to_rate_z(float alt_cm);
// rate_to_accel_z - calculates desired accel required to achieve the velocity target
void rate_to_accel_z(float vel_target_z);
// accel_to_throttle - alt hold's acceleration controller
void accel_to_throttle(float accel_target_z);
/*
/// get_loiter_position_to_velocity - loiter position controller
/// converts desired position held in _target vector to desired velocity
void get_position_to_velocity(float dt, float max_speed_cms);
@ -161,43 +182,55 @@ protected:
/// calculate_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void calculate_leash_length();
*/
// references to inertial nav and ahrs libraries
const AP_InertialNav& _inav;
const AP_AHRS& _ahrs;
const AC_AttitudeControl& _attitude_control;
const AP_InertialNav& _inav;
const AP_Motors& _motors;
AC_AttitudeControl& _attitude_control;
// references to pid controllers
// references to pid controllers and motors
APM_PI& _pi_alt_pos;
AC_PID& _pid_alt_rate;
AC_PID& _pid_alt_accel;
APM_PI& _pid_pos_lat;
APM_PI& _pid_pos_lon;
APM_PI& _pi_pos_lat;
APM_PI& _pi_pos_lon;
AC_PID& _pid_rate_lat;
AC_PID& _pid_rate_lon;
// parameters
AP_Float _speed_cms; // maximum horizontal speed in cm/s while in loiter
AP_Float _speed_up_cms; // climb speed target in cm/s
AP_Float _speed_down_cms; // descent speed target in cm/s
uint8_t _step; // used to decide which portion of loiter controller to run during this iteration
uint32_t _last_update; // system time of last update_position_controller call
float _dt; // time difference since last update_position_controller call
AP_Float _throttle_hover; // estimated throttle required to maintain a level hover
// 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)
uint8_t _step; // used to decide which portion of position controller to run during this iteration
float _speed_cms; // max horizontal speed in cm/s
float _vel_z_min; // min climb rate (i.e. max descent rate) in cm/s
float _vel_z_max; // max climb rate in cm/s
float _accel_cms; // max horizontal acceleration in cm/s/s
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;
// output from controller
int32_t _desired_roll; // fed to stabilize controllers at 50hz
int32_t _desired_pitch; // fed to stabilize controllers at 50hz
float _desired_roll; // desired roll angle in centi-degrees calculated by position controller
float _desired_pitch; // desired roll pitch in centi-degrees calculated by position controller
// position controller internal variables
Vector3f _target; // loiter's target location in cm from home
Vector3f _target_vel; // pilot's latest desired velocity in earth-frame
Vector3f _pos_target; // target location in cm from home
Vector3f _pos_error; // error between desired and actual position in cm
Vector3f _vel_target; // desired velocity in cm/s
Vector3f _vel_error; // error between desired and actual acceleration in cm/s. To-Do: x & y actually used?
Vector3f _vel_last; // previous iterations velocity in cm/s
float _vel_target_filt_z; // filtered target vertical velocity
Vector3f _accel_target; // desired acceleration in cm/s/s // To-Do: are xy actually required?
Vector3f _accel_error; // desired acceleration in cm/s/s // To-Do: are xy actually required?
float _leash; // horizontal leash length in cm. used to stop the pilot from pushing the target location too far from the current location
float _accel_cms; // maximum acceleration in cm/s/s
int16_t _lean_angle_max_cd; // maximum lean angle in centi-degrees
float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence
public:
// for logging purposes