/// update - update circle controller
void AC_Circle::update()
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
// update circle position at poscontrol update rate
if (dt >= _pos_control.get_dt_xy()) {
// double check dt is reasonable
if (dt >= 0.2f) {
dt = 0.0f;
}
// ramp angular velocity to maximum
if (_angular_vel < _angular_vel_max) {
_angular_vel += fabsf(_angular_accel) * dt;
_angular_vel = min(_angular_vel, _angular_vel_max);
}
if (_angular_vel > _angular_vel_max) {
_angular_vel -= fabsf(_angular_accel) * dt;
_angular_vel = max(_angular_vel, _angular_vel_max);
}
// update the target angle and total angle traveled
float angle_change = _angular_vel * dt;
_angle += angle_change;
_angle = wrap_PI(_angle);
_angle_total += angle_change;
// if the circle_radius is zero we are doing panorama so no need to update loiter target
if (!is_zero(_radius)) {
// calculate target position
Vector3f target;
target.x = _center.x + _radius * cosf(-_angle);
target.y = _center.y - _radius * sinf(-_angle);
target.z = _pos_control.get_alt_target();
// update position controller target
_pos_control.set_xy_target(target.x, target.y);
// heading is 180 deg from vehicles target position around circle
_yaw = wrap_PI(_angle-PI) * AC_CIRCLE_DEGX100;
}else{
// set target position to center
Vector3f target;
target.x = _center.x;
target.y = _center.y;
target.z = _pos_control.get_alt_target();
// update position controller target
_pos_control.set_xy_target(target.x, target.y);
// heading is same as _angle but converted to centi-degrees
_yaw = _angle * AC_CIRCLE_DEGX100;
}
// update position controller
_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f, false);
}
}
// Command a Quaternion attitude with feedforward and smoothing
void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain)
{
// 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);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
Quaternion attitude_error_quat = _attitude_target_quat.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle;
attitude_error_quat.to_axis_angle(attitude_error_angle);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_attitude_target_ang_vel.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), smoothing_gain, get_accel_roll_max_radss(), _attitude_target_ang_vel.x, _dt);
_attitude_target_ang_vel.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), smoothing_gain, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y, _dt);
_attitude_target_ang_vel.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), smoothing_gain, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z, _dt);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} else {
_attitude_target_quat = attitude_desired_quat;
// 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);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// init_start_angle - sets the starting angle around the circle and initialises the angle_total
// if use_heading is true the vehicle's heading will be used to init the angle causing minimum yaw movement
// if use_heading is false the vehicle's position from the center will be used to initialise the angle
void AC_Circle::init_start_angle(bool use_heading)
{
// initialise angle total
_angle_total = 0;
// if the radius is zero we are doing panorama so init angle to the current heading
if (_radius <= 0) {
_angle = _ahrs.yaw;
return;
}
// if use_heading is true
if (use_heading) {
_angle = wrap_PI(_ahrs.yaw-PI);
} else {
// if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
const Vector3f &curr_pos = _inav.get_position();
if (is_equal(curr_pos.x,_center.x) && is_equal(curr_pos.y,_center.y)) {
_angle = wrap_PI(_ahrs.yaw-PI);
} else {
// get bearing from circle center to vehicle in radians
float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
_angle = wrap_PI(bearing_rad);
}
}
}
/// 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)
{
// 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 hovering
const float pilot_cos_pitch_target = constrain_float(cosf(euler_pitch_angle), 0.5f, 1.0f);
const float pilot_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(euler_roll_angle)/pilot_cos_pitch_target;
const float pilot_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(euler_pitch_angle);
// rotate acceleration vectors input to lat/lon frame
_desired_accel.x = (pilot_accel_fwd_cms*_ahrs.cos_yaw() - pilot_accel_rgt_cms*_ahrs.sin_yaw());
_desired_accel.y = (pilot_accel_fwd_cms*_ahrs.sin_yaw() + pilot_accel_rgt_cms*_ahrs.cos_yaw());
// difference between where we think we should be and where we want to be
Vector2f angle_error(wrap_PI(euler_roll_angle - _predicted_euler_angle.x), wrap_PI(euler_pitch_angle - _predicted_euler_angle.y));
// calculate the angular velocity that we would expect given our desired and predicted attitude
_attitude_control.input_shaping_rate_predictor(angle_error, _predicted_euler_rate, dt);
// update our predicted attitude based on our predicted angular velocity
_predicted_euler_angle += _predicted_euler_rate * dt;
// convert our predicted attitude to an acceleration vector assuming we are hovering
const float pilot_predicted_cos_pitch_target = cosf(_predicted_euler_angle.y);
const float pilot_predicted_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.x)/pilot_predicted_cos_pitch_target;
const float pilot_predicted_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.y);
// rotate acceleration vectors input to lat/lon frame
_predicted_accel.x = (pilot_predicted_accel_fwd_cms*_ahrs.cos_yaw() - pilot_predicted_accel_rgt_cms*_ahrs.sin_yaw());
_predicted_accel.y = (pilot_predicted_accel_fwd_cms*_ahrs.sin_yaw() + pilot_predicted_accel_rgt_cms*_ahrs.cos_yaw());
}
TEST(MathWrapTest, AnglePI)
{
const float accuracy = 1.0e-5;
EXPECT_NEAR(M_PI, wrap_PI(M_PI), accuracy);
EXPECT_NEAR(0.f, wrap_PI(M_2PI), accuracy);
EXPECT_NEAR(0, wrap_PI(M_PI * 10), accuracy);
}
//
// drift_correction_yaw - yaw drift correction using the compass
//
void
AP_AHRS_MPU6000::drift_correction_yaw(void)
{
static float yaw_last_uncorrected = HEADING_UNKNOWN;
static float yaw_corrected = HEADING_UNKNOWN;
float yaw_delta = 0;
bool new_value = false;
float yaw_error;
static float heading;
// we assume the DCM matrix is completely uncorrect for yaw
// retrieve how much heading has changed according to non-corrected dcm
if( yaw_last_uncorrected != HEADING_UNKNOWN ) {
yaw_delta = wrap_PI(yaw - yaw_last_uncorrected); // the change in heading according to the gyros only since the last interation
yaw_last_uncorrected = yaw;
}
// initialise yaw_corrected (if necessary)
if( yaw_corrected != HEADING_UNKNOWN ) {
yaw_corrected = yaw;
}else{
yaw_corrected = wrap_PI(yaw_corrected + yaw_delta); // best guess of current yaw is previous iterations corrected yaw + yaw change from gyro
_dcm_matrix.from_euler(roll, pitch, yaw_corrected); // rebuild dcm matrix with best guess at current yaw
}
// if we have new compass data
if( _compass && _compass->use_for_yaw() ) {
if (_compass->last_update != _compass_last_update) {
_compass_last_update = _compass->last_update;
heading = _compass->calculate_heading(_dcm_matrix);
if( !_have_initial_yaw ) {
yaw_corrected = heading;
_have_initial_yaw = true;
_dcm_matrix.from_euler(roll, pitch, yaw_corrected); // rebuild dcm matrix with best guess at current yaw
}
new_value = true;
}
}
// perform the yaw correction
if( new_value ) {
yaw_error = yaw_error_compass();
// the yaw error is a vector in earth frame
//Vector3f error = Vector3f(0,0, yaw_error);
// convert the error vector to body frame
//error = _dcm_matrix.mul_transpose(error);
// Update the differential component to reduce the error (it´s like a P control)
yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get()); // probably completely wrong
// rebuild the dcm matrix yet again
_dcm_matrix.from_euler(roll, pitch, yaw_corrected);
}
}
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_angle = radians(euler_yaw_angle_cd*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);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad();
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 and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt);
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt);
_attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), smoothing_gain, euler_accel.z, _attitude_target_euler_rate.z, _dt);
if (slew_yaw) {
_attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
}
// 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.
_attitude_target_euler_angle.x = euler_roll_angle;
_attitude_target_euler_angle.y = euler_pitch_angle;
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), -get_slew_yaw_rads()*_dt, get_slew_yaw_rads()*_dt);
// Update attitude target from constrained angle error
_attitude_target_euler_angle.z = wrap_PI(angle_error + _attitude_target_euler_angle.z);
} else {
_attitude_target_euler_angle.z = euler_yaw_angle;
}
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// 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);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
//
// drift_correction_yaw - yaw drift correction using the compass
// we have no way to update the dmp with it's actual heading from our
// compass instead we use the yaw_corrected variable to hold what we think
// is the real heading we also record what the dmp said it's last heading
// was in the yaw_last_uncorrected variable so that on the next iteration we
// can add the change in yaw to our estimate
//
void
AP_AHRS_MPU6000::drift_correction_yaw(void)
{
static float yaw_corrected = HEADING_UNKNOWN;
static float last_dmp_yaw = HEADING_UNKNOWN;
// roll pitch and yaw values from dmp
float dmp_roll, dmp_pitch, dmp_yaw;
// change in yaw according to dmp
float yaw_delta;
// difference between heading and corrected yaw
float yaw_error;
static float heading;
// get uncorrected yaw values from dmp
_mpu6000->quaternion.to_euler(&dmp_roll, &dmp_pitch, &dmp_yaw);
// initialise headings on first iteration
if( yaw_corrected == HEADING_UNKNOWN ) {
yaw_corrected = dmp_yaw;
last_dmp_yaw = dmp_yaw;
}
// change in yaw according to dmp
yaw_delta = wrap_PI(dmp_yaw - last_dmp_yaw);
yaw_corrected = wrap_PI(yaw_corrected + yaw_delta);
last_dmp_yaw = dmp_yaw;
// rebuild dcm matrix
_dcm_matrix.from_euler(dmp_roll, dmp_pitch, yaw_corrected);
// if we have new compass data
if(_compass && _compass->use_for_yaw() ) {
if(_compass->last_update != _compass_last_update) {
_compass_last_update = _compass->last_update;
heading = _compass->calculate_heading(_dcm_matrix);
// if this is the first good compass reading then set yaw to this heading
if( !_have_initial_yaw ) {
_have_initial_yaw = true;
yaw_corrected = wrap_PI(heading);
}
// yaw correction based on compass
//yaw_error = yaw_error_compass();
yaw_error = wrap_PI(heading - yaw_corrected);
// shift the corrected yaw towards the compass heading a bit
yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get() * 0.1);
// rebuild the dcm matrix yet again
_dcm_matrix.from_euler(dmp_roll, dmp_pitch, yaw_corrected);
}
}
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
Vector3f att_error_euler_rad;
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
// Set roll/pitch attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
// Update roll/pitch attitude error.
att_error_euler_rad.x = wrap_PI(_att_target_euler_rad.x - _ahrs.roll);
att_error_euler_rad.y = wrap_PI(_att_target_euler_rad.y - _ahrs.pitch);
// Zero the roll and pitch feed-forward rate.
_att_target_euler_rate_rads.x = 0;
_att_target_euler_rate_rads.y = 0;
if (get_accel_yaw_max_radss() > 0.0f) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_and_error_yaw(_att_target_euler_rate_rads.z, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
} else {
// When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate
// is fed forward into the rate controller.
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
update_att_target_and_error_yaw(_att_target_euler_rate_rads.z, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
}
// Convert 321-intrinsic euler angle errors to a body-frame rotation vector
// NOTE: This results in an approximation of the attitude error based on a linearization about the current attitude
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_error_euler_rad, _att_error_rot_vec_rad);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
// Convert euler angle derivatives of desired attitude into a body-frame angular velocity vector for feedforward
// NOTE: This should be done about the desired attitude instead of about the vehicle attitude
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_target_euler_rate_rads, _att_target_ang_vel_rads);
// NOTE: A rotation of _att_target_ang_vel_rads from desired body frame to estimated body frame is possibly omitted here
// Add the angular velocity feedforward
_ang_vel_target_rads += _att_target_ang_vel_rads;
}
bool Plane::verify_takeoff()
{
if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
const float min_gps_speed = 5;
if (auto_state.takeoff_speed_time_ms == 0 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
gps.ground_speed() > min_gps_speed &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
auto_state.takeoff_speed_time_ms = millis();
}
if (auto_state.takeoff_speed_time_ms != 0 &&
millis() - auto_state.takeoff_speed_time_ms >= 2000) {
// once we reach sufficient speed for good GPS course
// estimation we save our current GPS ground course
// corrected for summed yaw to set the take off
// course. This keeps wings level until we are ready to
// rotate, and also allows us to cope with arbitrary
// compass errors for auto takeoff
float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err;
takeoff_course = wrap_PI(takeoff_course);
steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Holding course %ld at %.1fm/s (%.1f)",
steer_state.hold_course_cd,
(double)gps.ground_speed(),
(double)degrees(steer_state.locked_course_err));
}
}
if (steer_state.hold_course_cd != -1) {
// call navigation controller for heading hold
nav_controller->update_heading_hold(steer_state.hold_course_cd);
} else {
nav_controller->update_level_flight();
}
// see if we have reached takeoff altitude
int32_t relative_alt_cm = adjusted_relative_altitude_cm();
if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm",
(double)(relative_alt_cm*0.01f));
steer_state.hold_course_cd = -1;
auto_state.takeoff_complete = true;
next_WP_loc = prev_WP_loc = current_loc;
plane.complete_auto_takeoff();
// don't cross-track on completion of takeoff, as otherwise we
// can end up doing too sharp a turn
auto_state.next_wp_no_crosstrack = true;
return true;
} else {
return false;
}
}
void AC_AttitudeControl::update_att_target_roll(float euler_roll_rate_rads, float overshoot_max_rad)
{
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.x - _ahrs.roll), -overshoot_max_rad, overshoot_max_rad);
// Update attitude target from constrained angle error
_att_target_euler_rad.x = angle_error + _ahrs.roll;
// Increment the attitude target
_att_target_euler_rad.x += euler_roll_rate_rads * _dt;
_att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x);
}
void AC_AttitudeControl::update_att_target_pitch(float euler_pitch_rate_rads, float overshoot_max_rad)
{
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.y - _ahrs.pitch), -overshoot_max_rad, overshoot_max_rad);
// Update attitude target from constrained angle error
_att_target_euler_rad.y = angle_error + _ahrs.pitch;
// Increment the attitude target
_att_target_euler_rad.y += euler_pitch_rate_rads * _dt;
_att_target_euler_rad.y = wrap_PI(_att_target_euler_rad.y);
}
float AP_Landing_Deepstall::update_steering()
{
Location current_loc;
if ((!landing.ahrs.get_position(current_loc) || !landing.ahrs.healthy()) && !hold_level) {
// panic if no position source is available
// continue the stall but target just holding the wings held level as deepstall should be a minimal
// energy configuration on the aircraft, and if a position isn't available aborting would be worse
gcs().send_text(MAV_SEVERITY_CRITICAL, "Deepstall: Invalid data from AHRS. Holding level");
hold_level = true;
}
float desired_change = 0.0f;
if (!hold_level) {
uint32_t time = AP_HAL::millis();
float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) * 1e-3;
last_time = time;
Vector2f ab = location_diff(arc_exit, extended_approach);
ab.normalize();
Vector2f a_air = location_diff(arc_exit, current_loc);
crosstrack_error = a_air % ab;
float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
float nu1 = asinf(sine_nu1);
if (L1_i > 0) {
L1_xtrack_i += nu1 * L1_i / dt;
L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f);
nu1 += L1_xtrack_i;
}
desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw) / time_constant;
}
float yaw_rate = landing.ahrs.get_gyro().z;
float yaw_rate_limit_rps = radians(yaw_rate_limit);
float error = wrap_PI(constrain_float(desired_change, -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);
#ifdef DEBUG_PRINTS
gcs().send_text(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
(double)crosstrack_error,
(double)error,
(double)degrees(yaw_rate),
(double)location_diff(current_loc, landing_point).length());
#endif // DEBUG_PRINTS
return ds_PID.get_pid(error);
}
/*
Wrap AHRS yaw if in reverse - radians
*/
float AP_L1_Control::get_yaw()
{
if (_reverse) {
return wrap_PI(M_PI + _ahrs.yaw);
}
return _ahrs.yaw;
}
// calc_velocities - calculate angular velocity max and acceleration based on radius and rate
// this should be called whenever the radius or rate are changed
// initialises the yaw and current position around the circle
void AC_Circle::calc_velocities()
{
// if we are doing a panorama set the circle_angle to the current heading
if (_radius <= 0) {
_angular_vel_max = ToRad(_rate);
_angular_accel = _angular_vel_max; // reach maximum yaw velocity in 1 second
} else {
// set starting angle to current heading - 180 degrees
_angle = wrap_PI(_ahrs.yaw-PI);
// calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
float velocity_max = min(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius));
// angular_velocity in radians per second
_angular_vel_max = velocity_max/_radius;
_angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max);
// angular_velocity in radians per second
_angular_accel = _pos_control.get_accel_xy()/_radius;
if (_rate < 0.0f) {
_angular_accel = -_angular_accel;
}
}
// initialise angular velocity
_angular_vel = 0;
}
/*
* Adjusts the desired velocity based on output from the proximity sensor
*/
void AC_Avoid::adjust_velocity_proximity(const float kP, const float accel_cmss, Vector2f &desired_vel)
{
// exit immediately if proximity sensor is not present
if (_proximity.get_status() != AP_Proximity::Proximity_Good) {
return;
}
// exit immediately if no desired velocity
if (desired_vel.is_zero()) {
return;
}
// normalise desired velocity vector
Vector2f vel_dir = desired_vel.normalized();
// get angle of desired velocity
float heading_rad = atan2f(vel_dir.y, vel_dir.x);
// rotate desired velocity angle into body-frame angle
float heading_bf_rad = wrap_PI(heading_rad - _ahrs.yaw);
// get nearest object using body-frame angle and shorten desired velocity (which must remain in earth-frame)
float distance_m;
if (_proximity.get_horizontal_distance(degrees(heading_bf_rad), distance_m)) {
limit_velocity(kP, accel_cmss, desired_vel, vel_dir, MAX(distance_m*100.0f - 200.0f, 0.0f));
}
}
// update AHRS system
void Plane::ahrs_update()
{
update_soft_armed();
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// update hil before AHRS update
gcs().update_receive();
}
#endif
ahrs.update();
if (should_log(MASK_LOG_IMU)) {
DataFlash.Log_Write_IMU();
}
// calculate a scaled roll limit based on current pitch
roll_limit_cd = aparm.roll_limit_cd;
pitch_limit_min_cd = aparm.pitch_limit_min_cd;
if (!quadplane.tailsitter_active()) {
roll_limit_cd *= ahrs.cos_pitch();
pitch_limit_min_cd *= fabsf(ahrs.cos_roll());
}
// updated the summed gyro used for ground steering and
// auto-takeoff. Dot product of DCM.c with gyro vector gives earth
// frame yaw rate
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
// update inertial_nav for quadplane
quadplane.inertial_nav.update(G_Dt);
}
// calculate steering output to drive towards desired heading
void Mode::calc_steering_to_heading(float desired_heading_cd, bool reversed)
{
// calculate yaw error (in radians) and pass to steering angle controller
const float yaw_error = wrap_PI(radians((desired_heading_cd - ahrs.yaw_sensor) * 0.01f));
const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed);
g2.motors.set_steering(steering_out * 4500.0f);
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_angle_rad = radians(euler_yaw_angle_cd*0.01f);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
// Set attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.z = euler_yaw_angle_rad;
// If slew_yaw is enabled, constrain yaw target within get_slew_yaw_rads() of _ahrs.yaw
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.z - _ahrs.yaw), -get_slew_yaw_rads(), get_slew_yaw_rads());
// Update attitude target from constrained angle error
_att_target_euler_rad.z = angle_error + _ahrs.yaw;
}
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, Vector3f(0.0f,0.0f,0.0f));
// Keep euler derivative updated
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
// update AHRS system
void Plane::ahrs_update()
{
hal.util->set_soft_armed(arming.is_armed() &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// update hil before AHRS update
gcs_update();
}
#endif
ahrs.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_IMU();
}
// calculate a scaled roll limit based on current pitch
roll_limit_cd = aparm.roll_limit_cd * cosf(ahrs.pitch);
pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
// updated the summed gyro used for ground steering and
// auto-takeoff. Dot product of DCM.c with gyro vector gives earth
// frame yaw rate
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
// update inertial_nav for quadplane
quadplane.inertial_nav.update(G_Dt);
}
static void test_wrap_cd(void)
{
for (uint8_t i=0; i < ARRAY_SIZE(wrap_180_tests); i++) {
int32_t r = wrap_180_cd(wrap_180_tests[i].v);
if (r != wrap_180_tests[i].wv) {
hal.console->printf("wrap_180: v=%ld wv=%ld r=%ld\n",
(long)wrap_180_tests[i].v,
(long)wrap_180_tests[i].wv,
(long)r);
}
}
for (uint8_t i=0; i < ARRAY_SIZE(wrap_360_tests); i++) {
int32_t r = wrap_360_cd(wrap_360_tests[i].v);
if (r != wrap_360_tests[i].wv) {
hal.console->printf("wrap_360: v=%ld wv=%ld r=%ld\n",
(long)wrap_360_tests[i].v,
(long)wrap_360_tests[i].wv,
(long)r);
}
}
for (uint8_t i=0; i < ARRAY_SIZE(wrap_PI_tests); i++) {
float r = wrap_PI(wrap_PI_tests[i].v);
if (fabsf(r - wrap_PI_tests[i].wv) > 0.001f) {
hal.console->printf("wrap_PI: v=%f wv=%f r=%f\n",
wrap_PI_tests[i].v,
wrap_PI_tests[i].wv,
r);
}
}
hal.console->printf("wrap_cd tests done\n");
}
void Quaternion::to_axis_angle(Vector3f &v) {
float l = sqrt(sq(q2)+sq(q3)+sq(q4));
v = Vector3f(q2,q3,q4);
if(l >= 1.0e-12f) {
v /= l;
v *= wrap_PI(2.0f * atan2f(l,q1));
}
}
float AP_Landing_Deepstall::update_steering()
{
Location current_loc;
if (!landing.ahrs.get_position(current_loc)) {
// panic if no position source is available
// continue the but target just holding the wings held level as deepstall should be a minimal energy
// configuration on the aircraft, and if a position isn't available aborting would be worse
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Deepstall: No position available. Attempting to hold level");
memcpy(¤t_loc, &landing_point, sizeof(Location));
}
uint32_t time = AP_HAL::millis();
float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) / 1000.0;
last_time = time;
Vector2f ab = location_diff(arc_exit, extended_approach);
ab.normalize();
Vector2f a_air = location_diff(arc_exit, current_loc);
crosstrack_error = a_air % ab;
float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
float nu1 = asinf(sine_nu1);
if (L1_i > 0) {
L1_xtrack_i += nu1 * L1_i / dt;
L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f);
nu1 += L1_xtrack_i;
}
float desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw);
float yaw_rate = landing.ahrs.get_gyro().z;
float yaw_rate_limit_rps = radians(yaw_rate_limit);
float error = wrap_PI(constrain_float(desired_change / time_constant,
-yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);
#ifdef DEBUG_PRINTS
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
(double)crosstrack_error,
(double)error,
(double)degrees(yaw_rate),
(double)location_diff(current_loc, landing_point).length());
#endif // DEBUG_PRINTS
return ds_PID.get_pid(error);
}
/// update - update circle controller
void AC_Circle::update()
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
if (dt >= 0.2f) {
dt = 0.0f;
}
// ramp angular velocity to maximum
if (_angular_vel < _angular_vel_max) {
_angular_vel += fabsf(_angular_accel) * dt;
_angular_vel = MIN(_angular_vel, _angular_vel_max);
}
if (_angular_vel > _angular_vel_max) {
_angular_vel -= fabsf(_angular_accel) * dt;
_angular_vel = MAX(_angular_vel, _angular_vel_max);
}
// update the target angle and total angle traveled
float angle_change = _angular_vel * dt;
_angle += angle_change;
_angle = wrap_PI(_angle);
_angle_total += angle_change;
// if the circle_radius is zero we are doing panorama so no need to update loiter target
if (!is_zero(_radius)) {
// calculate target position
Vector3f target;
target.x = _center.x + _radius * cosf(-_angle);
target.y = _center.y - _radius * sinf(-_angle);
target.z = _pos_control.get_alt_target();
// update position controller target
_pos_control.set_xy_target(target.x, target.y);
// heading is from vehicle to center of circle
_yaw = get_bearing_cd(_inav.get_position(), _center);
} else {
// set target position to center
Vector3f target;
target.x = _center.x;
target.y = _center.y;
target.z = _pos_control.get_alt_target();
// update position controller target
_pos_control.set_xy_target(target.x, target.y);
// heading is same as _angle but converted to centi-degrees
_yaw = _angle * DEGX100;
}
// update position controller
_pos_control.update_xy_controller();
}
// returns true if sailboat should take a indirect navigation route to go upwind
// desired_heading should be in centi-degrees
bool Rover::sailboat_use_indirect_route(float desired_heading_cd) const
{
if (!g2.motors.has_sail()) {
return false;
}
// convert desired heading to radians
const float desired_heading_rad = radians(desired_heading_cd * 0.01f);
// check if desired heading is in the no go zone, if it is we can't go direct
return fabsf(wrap_PI(g2.windvane.get_absolute_wind_direction_rad() - desired_heading_rad)) <= radians(g2.sail_no_go);
}
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);
}
// Velocity Made Good, this is the speed we are traveling towards the desired destination
// only for logging at this stage
// https://en.wikipedia.org/wiki/Velocity_made_good
float Rover::sailboat_get_VMG() const
{
// return 0 if not heading towards waypoint
if (!control_mode->is_autopilot_mode()) {
return 0.0f;
}
float speed;
if (!g2.attitude_control.get_forward_speed(speed)) {
return 0.0f;
}
return (speed * cosf(wrap_PI(radians(g2.wp_nav.wp_bearing_cd() * 0.01f) - ahrs.yaw)));
}
// return a steering servo output from -1 to +1 given a heading in radians
float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max, bool motor_limit_left, bool motor_limit_right, float dt)
{
// calculate heading error (in radians)
const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw);
// Calculate the desired turn rate (in radians) from the angle error (also in radians)
float desired_rate = _steer_angle_p.get_p(yaw_error);
// limit desired_rate if a custom pivot turn rate is selected, otherwise use ATC_STR_RAT_MAX
if (is_positive(rate_max)) {
desired_rate = constrain_float(desired_rate, -rate_max, rate_max);
}
return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_angle_rad = radians(euler_yaw_angle_cd*0.01f);
Vector3f att_error_euler_rad;
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
// Set attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.z = euler_yaw_angle_rad;
// Update attitude error.
att_error_euler_rad.x = wrap_PI(_att_target_euler_rad.x - _ahrs.roll);
att_error_euler_rad.y = wrap_PI(_att_target_euler_rad.y - _ahrs.pitch);
att_error_euler_rad.z = wrap_PI(_att_target_euler_rad.z - _ahrs.yaw);
// Constrain the yaw angle error
if (slew_yaw) {
att_error_euler_rad.z = constrain_float(att_error_euler_rad.z,-get_slew_yaw_rads(),get_slew_yaw_rads());
}
// Convert 321-intrinsic euler angle errors to a body-frame rotation vector
// NOTE: This results in an approximation of the attitude error based on a linearization about the current attitude
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_error_euler_rad, _att_error_rot_vec_rad);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
// Keep euler derivative updated
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
{
Matrix3f att_to_rot_matrix; // rotation from the target body frame to the inertial frame.
att_to_quat.rotation_matrix(att_to_rot_matrix);
Vector3f att_to_thrust_vec = att_to_rot_matrix*Vector3f(0.0f,0.0f,1.0f);
Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
att_from_quat.rotation_matrix(att_from_rot_matrix);
Vector3f att_from_thrust_vec = att_from_rot_matrix*Vector3f(0.0f,0.0f,1.0f);
// the cross product of the desired and target thrust vector defines the rotation vector
Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;
// the dot product is used to calculate the angle between the target and desired thrust vectors
thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec,-1.0f,1.0f));
// Normalize the thrust rotation vector
float thrust_vector_length = thrust_vec_cross.length();
if(is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)){
thrust_vec_cross = Vector3f(0,0,1);
thrust_vec_dot = 0.0f;
}else{
thrust_vec_cross /= thrust_vector_length;
}
Quaternion thrust_vec_correction_quat;
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);
thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat;
// calculate the remaining rotation required after thrust vector is rotated
Quaternion heading_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat;
Vector3f rotation;
thrust_vec_correction_quat.to_axis_angle(rotation);
att_diff_angle.x = rotation.x;
att_diff_angle.y = rotation.y;
heading_quat.to_axis_angle(rotation);
att_diff_angle.z = rotation.z;
if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){
att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP());
heading_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z));
att_to_quat = att_from_quat*thrust_vec_correction_quat*heading_quat;
}
}
