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)); }