void AC_AttitudeControl::update_att_target_yaw(float euler_yaw_rate_rads, float overshoot_max_rad)
{
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.z - _ahrs.yaw), -overshoot_max_rad, overshoot_max_rad);
// Update attitude target from constrained angle error
_att_target_euler_rad.z = angle_error + _ahrs.yaw;
// Increment the attitude target
_att_target_euler_rad.z += euler_yaw_rate_rads * _dt;
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z);
}
// find next boundary point from an array of boundary points given the current index into that array
// returns true if a next point can be found
// current_index should be an index into the boundary_pts array
// start_angle is an angle (in radians), the search will sweep clockwise from this angle
// the index of the next point is returned in the next_index argument
// the angle to the next point is returned in the next_angle argument
bool AP_Beacon::get_next_boundary_point(const Vector2f* boundary_pts, uint8_t num_points, uint8_t current_index, float start_angle, uint8_t& next_index, float& next_angle)
{
// sanity check
if (boundary_pts == nullptr || current_index >= num_points) {
return false;
}
// get current point
Vector2f curr_point = boundary_pts[current_index];
// search through all points for next boundary point in a clockwise direction
float lowest_angle = M_PI_2;
float lowest_angle_relative = M_PI_2;
bool lowest_found = false;
uint8_t lowest_index = 0;
for (uint8_t i=0; i < num_points; i++) {
if (i != current_index) {
Vector2f vec = boundary_pts[i] - curr_point;
if (!vec.is_zero()) {
float angle = wrap_2PI(atan2f(vec.y, vec.x));
float angle_relative = wrap_2PI(angle - start_angle);
if ((angle_relative < lowest_angle_relative) || !lowest_found) {
lowest_angle = angle;
lowest_angle_relative = angle_relative;
lowest_index = i;
lowest_found = true;
}
}
}
}
// return results
if (lowest_found) {
next_index = lowest_index;
next_angle = lowest_angle;
}
return lowest_found;
}
TEST(MathWrapTest, Angle2PI)
{
const float accuracy = 1.0e-5;
EXPECT_NEAR(M_PI, wrap_2PI(M_PI), accuracy);
EXPECT_NEAR(0.f, wrap_2PI(M_2PI), accuracy);
EXPECT_NEAR(0.f, wrap_2PI(M_PI * 10), accuracy);
EXPECT_NEAR(0.f, wrap_2PI(0.f), accuracy);
EXPECT_NEAR(M_PI, wrap_2PI(-M_PI), accuracy);
EXPECT_NEAR(0, wrap_2PI(-M_2PI), accuracy);
}
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_rate = radians(euler_roll_rate_cds*0.01f);
float euler_pitch_rate = radians(euler_pitch_rate_cds*0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing
// the output rate towards the input rate.
_attitude_target_euler_rate.x = input_shaping_ang_vel(_attitude_target_euler_rate.x, euler_roll_rate, euler_accel.x);
_attitude_target_euler_rate.y = input_shaping_ang_vel(_attitude_target_euler_rate.y, euler_pitch_rate, euler_accel.y);
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
// Pitch angle is restricted to +- 85.0 degrees to avoid gimbal lock discontinuities.
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + euler_roll_rate*_dt);
_attitude_target_euler_angle.y = constrain_float(_attitude_target_euler_angle.y + euler_pitch_rate*_dt, radians(-85.0f), radians(85.0f));
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + euler_yaw_rate*_dt);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// handle user initiated tack while in acro mode
void Rover::sailboat_handle_tack_request_acro()
{
// set tacking heading target to the current angle relative to the true wind but on the new tack
sailboat.tacking = true;
sailboat.tack_heading_rad = wrap_2PI(ahrs.yaw + 2.0f * wrap_PI((g2.windvane.get_absolute_wind_direction_rad() - ahrs.yaw)));
}
// if we can't sail on the desired heading then we should pick the best heading that we can sail on
// this function assumes the caller has already checked sailboat_use_indirect_route(desired_heading_cd) returned true
float Rover::sailboat_calc_heading(float desired_heading_cd)
{
if (!g2.motors.has_sail()) {
return desired_heading_cd;
}
bool should_tack = false;
// check for user requested tack
uint32_t now = AP_HAL::millis();
if (sailboat.auto_tack_request_ms != 0) {
// set should_tack flag is user requested tack within last 0.5 sec
should_tack = ((now - sailboat.auto_tack_request_ms) < 500);
sailboat.auto_tack_request_ms = 0;
}
// calculate left and right no go headings looking upwind
const float left_no_go_heading_rad = wrap_2PI(g2.windvane.get_absolute_wind_direction_rad() + radians(g2.sail_no_go));
const float right_no_go_heading_rad = wrap_2PI(g2.windvane.get_absolute_wind_direction_rad() - radians(g2.sail_no_go));
// calculate current tack, Port if heading is left of no-go, STBD if right of no-go
Sailboat_Tack current_tack;
if (is_negative(g2.windvane.get_apparent_wind_direction_rad())) {
current_tack = TACK_PORT;
} else {
current_tack = TACK_STARBOARD;
}
// trigger tack if cross track error larger than waypoint_overshoot parameter
// this effectively defines a 'corridor' of width 2*waypoint_overshoot that the boat will stay within
if ((fabsf(g2.wp_nav.crosstrack_error()) >= g2.wp_nav.get_overshoot()) && !is_zero(g2.wp_nav.get_overshoot()) && !sailboat_tacking()) {
// make sure the new tack will reduce the cross track error
// if were on starboard tack we are traveling towards the left hand boundary
if (is_positive(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_STARBOARD)) {
should_tack = true;
}
// if were on port tack we are traveling towards the right hand boundary
if (is_negative(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_PORT)) {
should_tack = true;
}
}
// if tack triggered, calculate target heading
if (should_tack) {
gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking");
// calculate target heading for the new tack
switch (current_tack) {
case TACK_PORT:
sailboat.tack_heading_rad = right_no_go_heading_rad;
break;
case TACK_STARBOARD:
sailboat.tack_heading_rad = left_no_go_heading_rad;
break;
}
sailboat.tacking = true;
sailboat.auto_tack_start_ms = AP_HAL::millis();
}
// if we are tacking we maintain the target heading until the tack completes or times out
if (sailboat.tacking) {
// check if we have reached target
if (fabsf(wrap_PI(sailboat.tack_heading_rad - ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) {
sailboat_clear_tack();
} else if ((now - sailboat.auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) {
// tack has taken too long
gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking timed out");
sailboat_clear_tack();
}
// return tack target heading
return degrees(sailboat.tack_heading_rad) * 100.0f;
}
// return closest possible heading to wind
if (current_tack == TACK_PORT) {
return degrees(left_no_go_heading_rad) * 100.0f;
} else {
return degrees(right_no_go_heading_rad) * 100.0f;
}
}
// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds)
{
// convert from centidegrees on public interface to radians
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);
// store roll, pitch and passthroughs
// NOTE: this abuses yaw_rate_bf_rads
_passthrough_roll = roll_passthrough;
_passthrough_pitch = pitch_passthrough;
_passthrough_yaw = degrees(yaw_rate_bf_rads)*100.0f;
// set rate controller to use pass through
_flags_heli.flybar_passthrough = true;
// set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
_attitude_target_ang_vel.x = _ahrs.get_gyro().x;
_attitude_target_ang_vel.y = _ahrs.get_gyro().y;
// accel limit desired yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
float rate_change_rads = yaw_rate_bf_rads - _attitude_target_ang_vel.z;
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
_attitude_target_ang_vel.z += rate_change_rads;
} else {
_attitude_target_ang_vel.z = yaw_rate_bf_rads;
}
integrate_bf_rate_error_to_angle_errors();
_att_error_rot_vec_rad.x = 0;
_att_error_rot_vec_rad.y = 0;
// update our earth-frame angle targets
Vector3f att_error_euler_rad;
// convert angle error rotation vector into 321-intrinsic euler angle difference
// NOTE: this results an an approximation linearized about the vehicle's attitude
if (ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {
_attitude_target_euler_angle.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);
_attitude_target_euler_angle.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);
_attitude_target_euler_angle.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);
}
// handle flipping over pitch axis
if (_attitude_target_euler_angle.y > M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
if (_attitude_target_euler_angle.y < -M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(-M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
// convert body-frame angle errors to body-frame rate targets
_rate_target_ang_vel = update_ang_vel_target_from_att_error(_att_error_rot_vec_rad);
// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
_rate_target_ang_vel.x = _attitude_target_ang_vel.x;
_rate_target_ang_vel.y = _attitude_target_ang_vel.y;
// add desired target to yaw
_rate_target_ang_vel.z += _attitude_target_ang_vel.z;
_thrust_error_angle = norm(_att_error_rot_vec_rad.x, _att_error_rot_vec_rad.y);
}
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
{
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + radians(yaw_shift_cd*0.01f));
}
