// 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()
{
const Vector3f& curr_vel = _inav.get_velocity();
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 < _speed_down_cms) {
_vel_target.z = _speed_down_cms;
_limit.vel_down = true;
}
if (_vel_target.z > _speed_up_cms) {
_vel_target.z = _speed_up_cms;
_limit.vel_up = true;
}
// reset last velocity target to current target
if (_flags.reset_rate_to_accel_z) {
_vel_last.z = _vel_target.z;
}
// feed forward desired acceleration calculation
if (_dt > 0.0f) {
if (!_flags.freeze_ff_z) {
_accel_feedforward.z = (_vel_target.z - _vel_last.z)/_dt;
} else {
// stop the feed forward being calculated during a known discontinuity
_flags.freeze_ff_z = false;
}
} else {
_accel_feedforward.z = 0.0f;
}
// store this iteration's velocities for the next iteration
_vel_last.z = _vel_target.z;
// reset velocity error and filter if this controller has just been engaged
if (_flags.reset_rate_to_accel_z) {
// Reset Filter
_vel_error.z = 0;
_vel_error_filter.reset(0);
_flags.reset_rate_to_accel_z = false;
} else {
// calculate rate error and filter with cut off frequency of 2 Hz
_vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z);
}
// calculate p
p = _p_vel_z.kP() * _vel_error.z;
// consolidate and constrain target acceleration
desired_accel = _accel_feedforward.z + p;
desired_accel = constrain_int32(desired_accel, -32000, 32000);
// set target for accel based throttle controller
accel_to_throttle(desired_accel);
}
// 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 < _speed_down_cms) {
_vel_target.z = _speed_down_cms;
_limit.vel_down = true;
}
if (_vel_target.z > _speed_up_cms) {
_vel_target.z = _speed_up_cms;
_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 = _p_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);
}
// 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()
{
const Vector3f& curr_vel = _inav.get_velocity();
float p; // used to capture pid values for logging
// reset last velocity target to current target
if (_flags.reset_rate_to_accel_z) {
_vel_last.z = _vel_target.z;
}
// feed forward desired acceleration calculation
if (_dt > 0.0f) {
if (!_flags.freeze_ff_z) {
_accel_feedforward.z = (_vel_target.z - _vel_last.z)/_dt;
} else {
// stop the feed forward being calculated during a known discontinuity
_flags.freeze_ff_z = false;
}
} else {
_accel_feedforward.z = 0.0f;
}
// store this iteration's velocities for the next iteration
_vel_last.z = _vel_target.z;
// reset velocity error and filter if this controller has just been engaged
if (_flags.reset_rate_to_accel_z) {
// Reset Filter
_vel_error.z = 0;
_vel_error_filter.reset(0);
_flags.reset_rate_to_accel_z = false;
} else {
// calculate rate error and filter with cut off frequency of 2 Hz
_vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);
}
// calculate p
p = _p_vel_z.kP() * _vel_error.z;
// consolidate and constrain target acceleration
_accel_target.z = _accel_feedforward.z + p;
// set target for accel based throttle controller
accel_to_throttle(_accel_target.z);
}
