mirror of https://github.com/ArduPilot/ardupilot
AC_WPNav: Support changing update period
This commit is contained in:
parent
3c69d28237
commit
55e19bbf5b
|
@ -139,7 +139,7 @@ void AC_Loiter::soften_for_landing()
|
||||||
// dt should be the time (in seconds) since the last call to this function
|
// dt should be the time (in seconds) since the last call to this function
|
||||||
void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd)
|
void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd)
|
||||||
{
|
{
|
||||||
const float dt = _pos_control.get_dt();
|
const float dt = _attitude_control.get_dt();
|
||||||
// Convert from centidegrees on public interface to radians
|
// Convert from centidegrees on public interface to radians
|
||||||
const float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
|
const float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
|
||||||
const float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
|
const float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
|
||||||
|
@ -188,7 +188,7 @@ float AC_Loiter::get_angle_max_cd() const
|
||||||
/// run the loiter controller
|
/// run the loiter controller
|
||||||
void AC_Loiter::update(bool avoidance_on)
|
void AC_Loiter::update(bool avoidance_on)
|
||||||
{
|
{
|
||||||
calc_desired_velocity(_pos_control.get_dt(), avoidance_on);
|
calc_desired_velocity(avoidance_on);
|
||||||
_pos_control.update_xy_controller();
|
_pos_control.update_xy_controller();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -201,11 +201,13 @@ void AC_Loiter::sanity_check_params()
|
||||||
|
|
||||||
/// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
|
/// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
|
||||||
/// updated velocity sent directly to position controller
|
/// updated velocity sent directly to position controller
|
||||||
void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
|
void AC_Loiter::calc_desired_velocity(bool avoidance_on)
|
||||||
{
|
{
|
||||||
float ekfGndSpdLimit, ahrsControlScaleXY;
|
float ekfGndSpdLimit, ahrsControlScaleXY;
|
||||||
AP::ahrs().getControlLimits(ekfGndSpdLimit, ahrsControlScaleXY);
|
AP::ahrs().getControlLimits(ekfGndSpdLimit, ahrsControlScaleXY);
|
||||||
|
|
||||||
|
const float dt = _pos_control.get_dt();
|
||||||
|
|
||||||
// calculate a loiter speed limit which is the minimum of the value set by the LOITER_SPEED
|
// calculate a loiter speed limit which is the minimum of the value set by the LOITER_SPEED
|
||||||
// parameter and the value set by the EKF to observe optical flow limits
|
// parameter and the value set by the EKF to observe optical flow limits
|
||||||
float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit * 100.0f);
|
float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit * 100.0f);
|
||||||
|
@ -213,8 +215,8 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
|
||||||
|
|
||||||
float pilot_acceleration_max = angle_to_accel(get_angle_max_cd() * 0.01) * 100;
|
float pilot_acceleration_max = angle_to_accel(get_angle_max_cd() * 0.01) * 100;
|
||||||
|
|
||||||
// range check nav_dt
|
// range check dt
|
||||||
if (nav_dt < 0) {
|
if (is_negative(dt)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -223,19 +225,14 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
|
||||||
Vector2f desired_vel{desired_vel_3d.x,desired_vel_3d.y};
|
Vector2f desired_vel{desired_vel_3d.x,desired_vel_3d.y};
|
||||||
|
|
||||||
// update the desired velocity using our predicted acceleration
|
// update the desired velocity using our predicted acceleration
|
||||||
desired_vel.x += _predicted_accel.x * nav_dt;
|
desired_vel.x += _predicted_accel.x * dt;
|
||||||
desired_vel.y += _predicted_accel.y * nav_dt;
|
desired_vel.y += _predicted_accel.y * dt;
|
||||||
|
|
||||||
Vector2f loiter_accel_brake;
|
Vector2f loiter_accel_brake;
|
||||||
float desired_speed = desired_vel.length();
|
float desired_speed = desired_vel.length();
|
||||||
if (!is_zero(desired_speed)) {
|
if (!is_zero(desired_speed)) {
|
||||||
Vector2f desired_vel_norm = desired_vel / desired_speed;
|
Vector2f desired_vel_norm = desired_vel / desired_speed;
|
||||||
|
|
||||||
// TODO: consider using a velocity squared relationship like
|
|
||||||
// pilot_acceleration_max*(desired_speed/gnd_speed_limit_cms)^2;
|
|
||||||
// the drag characteristic of a multirotor should be examined to generate a curve
|
|
||||||
// we could add a expo function here to fine tune it
|
|
||||||
|
|
||||||
// calculate a drag acceleration based on the desired speed.
|
// calculate a drag acceleration based on the desired speed.
|
||||||
float drag_decel = pilot_acceleration_max * desired_speed / gnd_speed_limit_cms;
|
float drag_decel = pilot_acceleration_max * desired_speed / gnd_speed_limit_cms;
|
||||||
|
|
||||||
|
@ -244,17 +241,17 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
|
||||||
if (_desired_accel.is_zero()) {
|
if (_desired_accel.is_zero()) {
|
||||||
if ((AP_HAL::millis() - _brake_timer) > _brake_delay * 1000.0f) {
|
if ((AP_HAL::millis() - _brake_timer) > _brake_delay * 1000.0f) {
|
||||||
float brake_gain = _pos_control.get_vel_xy_pid().kP() * 0.5f;
|
float brake_gain = _pos_control.get_vel_xy_pid().kP() * 0.5f;
|
||||||
loiter_brake_accel = constrain_float(sqrt_controller(desired_speed, brake_gain, _brake_jerk_max_cmsss, nav_dt), 0.0f, _brake_accel_cmss);
|
loiter_brake_accel = constrain_float(sqrt_controller(desired_speed, brake_gain, _brake_jerk_max_cmsss, dt), 0.0f, _brake_accel_cmss);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
loiter_brake_accel = 0.0f;
|
loiter_brake_accel = 0.0f;
|
||||||
_brake_timer = AP_HAL::millis();
|
_brake_timer = AP_HAL::millis();
|
||||||
}
|
}
|
||||||
_brake_accel += constrain_float(loiter_brake_accel-_brake_accel, -_brake_jerk_max_cmsss*nav_dt, _brake_jerk_max_cmsss*nav_dt);
|
_brake_accel += constrain_float(loiter_brake_accel - _brake_accel, -_brake_jerk_max_cmsss * dt, _brake_jerk_max_cmsss * dt);
|
||||||
loiter_accel_brake = desired_vel_norm * _brake_accel;
|
loiter_accel_brake = desired_vel_norm * _brake_accel;
|
||||||
|
|
||||||
// update the desired velocity using the drag and braking accelerations
|
// update the desired velocity using the drag and braking accelerations
|
||||||
desired_speed = MAX(desired_speed-(drag_decel+_brake_accel)*nav_dt,0.0f);
|
desired_speed = MAX(desired_speed - (drag_decel + _brake_accel) * dt, 0.0f);
|
||||||
desired_vel = desired_vel_norm * desired_speed;
|
desired_vel = desired_vel_norm * desired_speed;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -275,7 +272,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
|
||||||
AC_Avoid *_avoid = AP::ac_avoid();
|
AC_Avoid *_avoid = AP::ac_avoid();
|
||||||
if (_avoid != nullptr) {
|
if (_avoid != nullptr) {
|
||||||
Vector3f avoidance_vel_3d{desired_vel.x, desired_vel.y, 0.0f};
|
Vector3f avoidance_vel_3d{desired_vel.x, desired_vel.y, 0.0f};
|
||||||
_avoid->adjust_velocity(avoidance_vel_3d, _pos_control.get_pos_xy_p().kP(), _accel_cmss, _pos_control.get_pos_z_p().kP(), _pos_control.get_max_accel_z_cmss(), nav_dt);
|
_avoid->adjust_velocity(avoidance_vel_3d, _pos_control.get_pos_xy_p().kP(), _accel_cmss, _pos_control.get_pos_z_p().kP(), _pos_control.get_max_accel_z_cmss(), dt);
|
||||||
desired_vel = Vector2f{avoidance_vel_3d.x, avoidance_vel_3d.y};
|
desired_vel = Vector2f{avoidance_vel_3d.x, avoidance_vel_3d.y};
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -285,7 +282,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
|
||||||
Vector2p target_pos = _pos_control.get_pos_target_cm().xy();
|
Vector2p target_pos = _pos_control.get_pos_target_cm().xy();
|
||||||
|
|
||||||
// update the target position using our predicted velocity
|
// update the target position using our predicted velocity
|
||||||
target_pos += (desired_vel * nav_dt).topostype();
|
target_pos += (desired_vel * dt).topostype();
|
||||||
|
|
||||||
// send adjusted feed forward acceleration and velocity back to the Position Controller
|
// send adjusted feed forward acceleration and velocity back to the Position Controller
|
||||||
_pos_control.set_pos_vel_accel_xy(target_pos, desired_vel, _desired_accel);
|
_pos_control.set_pos_vel_accel_xy(target_pos, desired_vel, _desired_accel);
|
||||||
|
|
|
@ -64,7 +64,7 @@ protected:
|
||||||
|
|
||||||
/// updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
|
/// updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
|
||||||
/// updated velocity sent directly to position controller
|
/// updated velocity sent directly to position controller
|
||||||
void calc_desired_velocity(float nav_dt, bool avoidance_on = true);
|
void calc_desired_velocity(bool avoidance_on = true);
|
||||||
|
|
||||||
// references and pointers to external libraries
|
// references and pointers to external libraries
|
||||||
const AP_InertialNav& _inav;
|
const AP_InertialNav& _inav;
|
||||||
|
|
Loading…
Reference in New Issue