AC_WPNav: Fix before squash

This commit is contained in:
Leonard Hall 2021-05-19 23:40:32 +09:30 committed by Andrew Tridgell
parent 9e9e139f99
commit 166f059fc2
3 changed files with 23 additions and 32 deletions

View File

@ -156,7 +156,7 @@ bool AC_Circle::update(float climb_rate_cms)
calc_velocities(false);
// calculate dt
float dt = _pos_control.get_dt();
const float dt = _pos_control.get_dt();
// ramp angular velocity to maximum
if (_angular_vel < _angular_vel_max) {
@ -189,12 +189,15 @@ bool AC_Circle::update(float climb_rate_cms)
}
// if the circle_radius is zero we are doing panorama so no need to update loiter target
Vector3f target;
Vector3f target {
_center.x,
_center.y,
target_z_cm
};
if (!is_zero(_radius)) {
// calculate target position
target.x = _center.x + _radius * cosf(-_angle);
target.y = _center.y - _radius * sinf(-_angle);
target.z = target_z_cm;
target.x += _radius * cosf(-_angle);
target.y += - _radius * sinf(-_angle);
// heading is from vehicle to center of circle
_yaw = get_bearing_cd(_inav.get_position(), _center);
@ -205,20 +208,15 @@ bool AC_Circle::update(float climb_rate_cms)
}
} else {
// set target position to center
target.x = _center.x;
target.y = _center.y;
target.z = target_z_cm;
// heading is same as _angle but converted to centi-degrees
_yaw = _angle * DEGX100;
}
// update position controller target
Vector3f vel = Vector3f();
_pos_control.input_pos_vel_accel_xy(target, vel, Vector3f());
Vector3f zero;
_pos_control.input_pos_vel_accel_xy(target, zero, zero);
if(_terrain_alt) {
_pos_control.input_pos_vel_accel_z(target, vel, Vector3f());
_pos_control.input_pos_vel_accel_z(target, zero, zero);
} else {
_pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms, false);
}

View File

@ -133,21 +133,21 @@ void AC_Loiter::soften_for_landing()
// also prevent I term build up in xy velocity controller. Note
// that this flag is reset on each loop, in update_xy_controller()
_pos_control.set_limit_xy();
_pos_control.set_externally_limited_xy();
}
/// set pilot desired acceleration in centi-degrees
// 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)
{
float dt = _pos_control.get_dt();
const float dt = _pos_control.get_dt();
// Convert from centidegrees on public interface to radians
const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
const float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
// convert our desired attitude to an acceleration vector assuming we are not accelerating vertically
Vector3f desired_euler = Vector3f{euler_roll_angle, euler_pitch_angle, _ahrs.yaw};
Vector3f desired_accel = _pos_control.lean_angles_to_accel(desired_euler);
const Vector3f desired_euler {euler_roll_angle, euler_pitch_angle, _ahrs.yaw};
const Vector3f desired_accel = _pos_control.lean_angles_to_accel(desired_euler);
_desired_accel.x = desired_accel.x;
_desired_accel.y = desired_accel.y;
@ -162,8 +162,8 @@ void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float
_predicted_euler_angle += _predicted_euler_rate * dt;
// convert our predicted attitude to an acceleration vector assuming we are not accelerating vertically
Vector3f predicted_euler = Vector3f{_predicted_euler_angle.x, _predicted_euler_angle.y, _ahrs.yaw};
Vector3f predicted_accel = _pos_control.lean_angles_to_accel(predicted_euler);
const Vector3f predicted_euler {_predicted_euler_angle.x, _predicted_euler_angle.y, _ahrs.yaw};
const Vector3f predicted_accel = _pos_control.lean_angles_to_accel(predicted_euler);
_predicted_accel.x = predicted_accel.x;
_predicted_accel.y = predicted_accel.y;
@ -187,13 +187,10 @@ float AC_Loiter::get_angle_max_cd() const
/// run the loiter controller
void AC_Loiter::update(bool avoidance_on)
{
// calculate dt
float dt = _pos_control.get_dt();
// initialise pos controller speed and acceleration
_pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss, _accel_cmss);
calc_desired_velocity(dt, avoidance_on);
calc_desired_velocity(_pos_control.get_dt(), avoidance_on);
_pos_control.update_xy_controller();
}
@ -228,7 +225,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
// get loiters desired velocity from the position controller where it is being stored.
const Vector3f &desired_vel_3d = _pos_control.get_vel_desired_cms();
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
desired_vel.x += _predicted_accel.x * nav_dt;
@ -292,8 +289,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
Vector2f target_pos{target_pos_3d.x, target_pos_3d.y};
// update the target position using our predicted velocity
target_pos.x += desired_vel.x * nav_dt;
target_pos.y += desired_vel.y * nav_dt;
target_pos += desired_vel * nav_dt;
// 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);

View File

@ -365,7 +365,7 @@ void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_xy()
_pos_control.init_xy_controller();
// get current and target locations
const Vector3f& curr_pos = _pos_control.get_pos_target_cm();
const Vector3f& curr_pos = _inav.get_position();
// shift origin and destination horizontally
_origin.x = curr_pos.x;
@ -538,11 +538,8 @@ bool AC_WPNav::update_wpnav()
{
bool ret = true;
// get dt from pos controller
float dt = _pos_control.get_dt();
// advance the target if necessary
if (!advance_wp_target_along_track(dt)) {
if (!advance_wp_target_along_track(_pos_control.get_dt())) {
// To-Do: handle inability to advance along track (probably because of missing terrain data)
ret = false;
}