/// update_velocity_controller_xy - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xy() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xy(float ekfNavVelGainScaler)
{
// capture time since last iteration
uint32_t now = AP_HAL::millis();
float dt = (now - _last_update_xy_ms)*0.001f;
// call xy controller
if (dt >= get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// check for ekf xy position reset
check_for_ekf_xy_reset();
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// apply desired velocity request to position target
desired_vel_to_pos(dt);
// run position controller's position error to desired velocity step
pos_to_rate_xy(XY_MODE_POS_LIMITED_AND_VEL_FF, dt, ekfNavVelGainScaler);
// run velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler);
// run acceleration to lean angle step
accel_to_lean_angles(dt, ekfNavVelGainScaler, false);
// update xy update time
_last_update_xy_ms = now;
}
}
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
void AC_PosControl::update_xy_controller(xy_mode mode, float ekfNavVelGainScaler, bool use_althold_lean_angle)
{
// compute dt
uint32_t now = AP_HAL::millis();
float dt = (now - _last_update_xy_ms)*0.001f;
_last_update_xy_ms = now;
// sanity check dt - expect to be called faster than ~5hz
if (dt > POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f) {
dt = 0.0f;
}
// check for ekf xy position reset
check_for_ekf_xy_reset();
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// translate any adjustments from pilot to loiter target
desired_vel_to_pos(dt);
// run position controller's position error to desired velocity step
pos_to_rate_xy(mode, dt, ekfNavVelGainScaler);
// run position controller's velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler);
// run position controller's acceleration to lean angle step
accel_to_lean_angles(dt, ekfNavVelGainScaler, use_althold_lean_angle);
}
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xyz() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
{
// capture time since last iteration
uint32_t now = hal.scheduler->millis();
float dt = (now - _last_update_xy_ms) / 1000.0f;
// sanity check dt - expect to be called faster than ~5hz
if (dt >= POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f) {
dt = 0.0f;
}
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// apply desired velocity request to position target
desired_vel_to_pos(dt);
// run position controller's position error to desired velocity step
pos_to_rate_xy(XY_MODE_POS_LIMITED_AND_VEL_FF, dt, ekfNavVelGainScaler);
// run velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler);
// run acceleration to lean angle step
accel_to_lean_angles(dt, ekfNavVelGainScaler, false);
// update altitude target
set_alt_target_from_climb_rate(_vel_desired.z, dt, false);
// run z-axis position controller
update_z_controller();
// record update time
_last_update_xy_ms = now;
}
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
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 (!_flags.keep_xy_I_terms) {
reset_I_xy();
}
_xy_step = 0;
}
// reset keep_xy_I_term flag in case it has been set
_flags.keep_xy_I_terms = false;
// 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;
_xy_step = 0;
}
// run loiter steps
switch (_xy_step) {
case 0:
// capture time since last iteration
_dt_xy = (now - _last_update_ms) / 1000.0f;
_last_update_ms = now;
// translate any adjustments from pilot to loiter target
desired_vel_to_pos(_dt_xy);
_xy_step++;
break;
case 1:
// run position controller's position error to desired velocity step
pos_to_rate_xy(use_desired_velocity,_dt_xy);
_xy_step++;
break;
case 2:
// run position controller's velocity to acceleration step
rate_to_accel_xy(_dt_xy);
_xy_step++;
break;
case 3:
// run position controller's acceleration to lean angle step
accel_to_lean_angles();
_xy_step++;
break;
}
}
int pos_controller_old::update_controller(float dt)
{
// if user released stick, or stick moving toward set point.
// apply a extra braking until velocity decreased to a acceptable value, for at most 1 second.
bool stick_released = fabs(desired_velocity_earth[0]) < 0.01f && fabs(desired_velocity_earth[1]) < 0.01f;
if (stick_released)
release_stick_timer += dt;
else
release_stick_timer = 0;
min_braking_speed = 1.0f * 1.0f;
float delta[2] = {setpoint[0] - pos[0], setpoint[1] - pos[1]};
if ( (velocity[0]*velocity[0] + velocity[1]*velocity[1] > min_braking_speed) && // speed fast enough(1m/s)?
(
(stick_released && release_stick_timer < 1.0f) // released stick? less than 1 second?
|| (delta[0]*desired_velocity_earth[0] + delta[1]*desired_velocity_earth[1] < 0) // moving toward set point?
)
)
{
// braking is based on a 0.05hz low pass filter that move set point toward current position
float alpha = dt / (dt + 1.0f/(2*PI * 0.05f));
setpoint[0] = alpha * pos[0] + (1-alpha) * setpoint[0];
setpoint[1] = alpha * pos[1] + (1-alpha) * setpoint[1];
}
move_desire_pos(dt);
pos_to_rate(dt);
rate_to_accel(dt);
accel_to_lean_angles();
#ifdef WIN32
fprintf(f, "%.3f,%f,%f,%f,%f\r\n", (GetTickCount()-tick)/1000.0f, velocity[0],target_velocity[0], pid[0][0], pid[0][1]);
fflush(f);
#endif
return 0;
}
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xyz() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xyz()
{
// capture time since last iteration
uint32_t now = hal.scheduler->millis();
float dt_xy = (now - _last_update_vel_xyz_ms) / 1000.0f;
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// we will run the horizontal component every 4th iteration (i.e. 50hz on Pixhawk, 20hz on APM)
if (dt_xy >= POSCONTROL_VEL_UPDATE_TIME) {
// record update time
_last_update_vel_xyz_ms = now;
// sanity check dt
if (dt_xy >= POSCONTROL_ACTIVE_TIMEOUT_MS) {
dt_xy = 0.0f;
}
// apply desired velocity request to position target
desired_vel_to_pos(dt_xy);
// run position controller's position error to desired velocity step
pos_to_rate_xy(true, dt_xy);
// run velocity to acceleration step
rate_to_accel_xy(dt_xy);
// run acceleration to lean angle step
accel_to_lean_angles();
}
// update altitude target
set_alt_target_from_climb_rate(_vel_desired.z, _dt);
// run z-axis position controller
update_z_controller();
}
/// run horizontal position controller correcting position and velocity
/// converts position (_pos_target) to target velocity (_vel_target)
/// desired velocity (_vel_desired) is combined into final target velocity
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::run_xy_controller(float dt)
{
float ekfGndSpdLimit, ekfNavVelGainScaler;
AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
Vector3f curr_pos = _inav.get_position();
float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF
// avoid divide by zero
if (kP <= 0.0f) {
_vel_target.x = 0.0f;
_vel_target.y = 0.0f;
}else{
// calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
_pos_error.y = _pos_target.y - curr_pos.y;
// Constrain _pos_error and target position
// Constrain the maximum length of _vel_target to the maximum position correction velocity
// TODO: replace the leash length with a user definable maximum position correction
if (limit_vector_length(_pos_error.x, _pos_error.y, _leash))
{
_pos_target.x = curr_pos.x + _pos_error.x;
_pos_target.y = curr_pos.y + _pos_error.y;
}
_vel_target = sqrt_controller(_pos_error, kP, _accel_cms);
}
// add velocity feed-forward
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
// the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
// check if vehicle velocity is being overridden
if (_flags.vehicle_horiz_vel_override) {
_flags.vehicle_horiz_vel_override = false;
} else {
_vehicle_horiz_vel.x = _inav.get_velocity().x;
_vehicle_horiz_vel.y = _inav.get_velocity().y;
}
// calculate velocity error
_vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;
_vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;
// TODO: constrain velocity error and velocity target
// call pi controller
_pid_vel_xy.set_input(_vel_error);
// get p
vel_xy_p = _pid_vel_xy.get_p();
// update i term if we have not hit the accel or throttle limits OR the i term will reduce
// TODO: move limit handling into the PI and PID controller
if (!_limit.accel_xy && !_motors.limit.throttle_upper) {
vel_xy_i = _pid_vel_xy.get_i();
} else {
vel_xy_i = _pid_vel_xy.get_i_shrink();
}
// get d
vel_xy_d = _pid_vel_xy.get_d();
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;
accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;
// reset accel to current desired acceleration
if (_flags.reset_accel_to_lean_xy) {
_accel_target_filter.reset(Vector2f(accel_target.x, accel_target.y));
_flags.reset_accel_to_lean_xy = false;
}
// filter correction acceleration
_accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
_accel_target_filter.apply(accel_target, dt);
// pass the correction acceleration to the target acceleration output
_accel_target.x = _accel_target_filter.get().x;
_accel_target.y = _accel_target_filter.get().y;
// Add feed forward into the target acceleration output
_accel_target.x += _accel_desired.x;
_accel_target.y += _accel_desired.y;
// the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles
// limit acceleration using maximum lean angles
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
float accel_max = MIN(GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)), POSCONTROL_ACCEL_XY_MAX);
_limit.accel_xy = limit_vector_length(_accel_target.x, _accel_target.y, accel_max);
// update angle targets that will be passed to stabilize controller
accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
}
