/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
{
// range check nav_dt
if( nav_dt < 0 ) {
return;
}
// check loiter speed and avoid divide by zero
if( _loiter_speed_cms < WPNAV_LOITER_SPEED_MIN) {
_loiter_speed_cms = WPNAV_LOITER_SPEED_MIN;
_loiter_accel_cms = _loiter_speed_cms/2.0f;
}
// rotate pilot input to lat/lon frame
Vector2f desired_accel;
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());
// calculate the difference
Vector2f des_accel_diff = (desired_accel - _loiter_desired_accel);
// constrain and scale the desired acceleration
float des_accel_change_total = pythagorous2(des_accel_diff.x, des_accel_diff.y);
float accel_change_max = _loiter_jerk_max_cmsss * nav_dt;
if (des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f) {
des_accel_diff.x = accel_change_max * des_accel_diff.x/des_accel_change_total;
des_accel_diff.y = accel_change_max * des_accel_diff.y/des_accel_change_total;
}
// adjust the desired acceleration
_loiter_desired_accel += des_accel_diff;
// get pos_control's feed forward velocity
Vector3f desired_vel = _pos_control.get_desired_velocity();
// add pilot commanded acceleration
desired_vel.x += _loiter_desired_accel.x * nav_dt;
desired_vel.y += _loiter_desired_accel.y * nav_dt;
// reduce velocity with fake wind resistance
if (_pilot_accel_fwd_cms != 0.0f || _pilot_accel_rgt_cms != 0.0f) {
desired_vel.x -= (_loiter_accel_cms)*nav_dt*desired_vel.x/_loiter_speed_cms;
desired_vel.y -= (_loiter_accel_cms)*nav_dt*desired_vel.y/_loiter_speed_cms;
} else {
desired_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.x/_loiter_speed_cms;
if(desired_vel.x > 0 ) {
desired_vel.x = max(desired_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
} else if(desired_vel.x < 0) {
desired_vel.x = min(desired_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}
desired_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.y/_loiter_speed_cms;
if(desired_vel.y > 0 ) {
desired_vel.y = max(desired_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
} else if(desired_vel.y < 0) {
desired_vel.y = min(desired_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}
}
// send adjusted feed forward velocity back to position controller
_pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y);
}
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
// sanity check angle max parameter
aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);
// limit max lean angle
angle_max = constrain_float(angle_max, 1000, aparm.angle_max);
// scale roll_in, pitch_in to ANGLE_MAX parameter range
float scaler = aparm.angle_max/(float)ROLL_PITCH_INPUT_MAX;
roll_in *= scaler;
pitch_in *= scaler;
// do circular limit
float total_in = pythagorous2((float)pitch_in, (float)roll_in);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// do lateral tilt to euler roll conversion
roll_in = (18000/M_PI_F) * atanf(cosf(pitch_in*(M_PI_F/18000))*tanf(roll_in*(M_PI_F/18000)));
// return
roll_out = roll_in;
pitch_out = pitch_in;
}
void Copter::rtl_build_path()
{
// origin point is our stopping point
pos_control.get_stopping_point_xy(rtl_path.origin_point);
pos_control.get_stopping_point_z(rtl_path.origin_point);
// set return target to nearest rally point or home position
#if AC_RALLY == ENABLED
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0);
rtl_path.return_target = pv_location_to_vector(rally_point);
#else
rtl_path.return_target = pv_location_to_vector(ahrs.get_home());
#endif
Vector3f return_vector = rtl_path.return_target-rtl_path.origin_point;
float rtl_return_dist = pythagorous2(return_vector.x, return_vector.y);
// compute return altitude
rtl_path.return_target.z = rtl_compute_return_alt_above_origin(rtl_return_dist);
// climb target is above our origin point at the return altitude
rtl_path.climb_target.x = rtl_path.origin_point.x;
rtl_path.climb_target.y = rtl_path.origin_point.y;
rtl_path.climb_target.z = rtl_path.return_target.z;
// descent target is below return target at rtl_alt_final
rtl_path.descent_target.x = rtl_path.return_target.x;
rtl_path.descent_target.y = rtl_path.return_target.y;
rtl_path.descent_target.z = pv_alt_above_origin(g.rtl_alt_final);
// set land flag
rtl_path.land = g.rtl_alt_final <= 0;
}
/**
* get_velocity_xy - returns the current horizontal velocity in cm/s
*
* @returns the current horizontal velocity in cm/s
*/
float AP_InertialNav_NavEKF::get_velocity_xy() const
{
if (_ahrs.have_inertial_nav()) {
return pythagorous2(_velocity_cm.x, _velocity_cm.y);
}
return AP_InertialNav::get_velocity_xy();
}
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
void AC_WPNav::calculate_wp_leash_length()
{
// length of the unit direction vector in the horizontal
float pos_delta_unit_xy = pythagorous2(_pos_delta_unit.x, _pos_delta_unit.y);
float pos_delta_unit_z = fabsf(_pos_delta_unit.z);
float speed_z;
float leash_z;
if (_pos_delta_unit.z >= 0) {
speed_z = _wp_speed_up_cms;
leash_z = _pos_control.get_leash_up_z();
}else{
speed_z = _wp_speed_down_cms;
leash_z = _pos_control.get_leash_down_z();
}
// calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel
if(pos_delta_unit_z == 0 && pos_delta_unit_xy == 0){
_track_accel = 0;
_track_speed = 0;
_track_leash_length = WPNAV_MIN_LEASH_LENGTH;
}else if(_pos_delta_unit.z == 0){
_track_accel = _wp_accel_cms/pos_delta_unit_xy;
_track_speed = _wp_speed_cms/pos_delta_unit_xy;
_track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy;
}else if(pos_delta_unit_xy == 0){
_track_accel = WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z;
_track_speed = speed_z/pos_delta_unit_z;
_track_leash_length = leash_z/pos_delta_unit_z;
}else{
_track_accel = min(WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy);
_track_speed = min(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);
_track_leash_length = min(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy);
}
}
/*
set HIL (hardware in the loop) status for a GPS instance
*/
void
AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
Location &_location, Vector3f &_velocity, uint8_t _num_sats,
uint16_t hdop, bool _have_vertical_velocity)
{
if (instance >= GPS_MAX_INSTANCES) {
return;
}
uint32_t tnow = hal.scheduler->millis();
GPS_State &istate = state[instance];
istate.status = _status;
istate.location = _location;
istate.location.options = 0;
istate.velocity = _velocity;
istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y);
istate.ground_course_cd = degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL;
istate.hdop = hdop;
istate.num_sats = _num_sats;
istate.have_vertical_velocity = _have_vertical_velocity;
istate.last_gps_time_ms = tnow;
istate.time_week = time_epoch_ms / (86400*7*(uint64_t)1000);
istate.time_week_ms = time_epoch_ms - istate.time_week*(86400*7*(uint64_t)1000);
timing[instance].last_message_time_ms = tnow;
timing[instance].last_fix_time_ms = tnow;
_type[instance].set(GPS_TYPE_HIL);
}
// update AHRS system
void Rover::ahrs_update()
{
hal.util->set_soft_armed(arming.is_armed() &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before AHRS update
gcs_update();
#endif
// when in reverse we need to tell AHRS not to assume we are a
// 'fly forward' vehicle, otherwise it will see a large
// discrepancy between the mag and the GPS heading and will try to
// correct for it, leading to a large yaw error
ahrs.set_fly_forward(!in_reverse);
ahrs.update();
// if using the EKF get a speed update now (from accelerometers)
Vector3f velocity;
if (ahrs.get_velocity_NED(velocity)) {
ground_speed = pythagorous2(velocity.x, velocity.y);
}
if (should_log(MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude();
if (should_log(MASK_LOG_IMU)) {
DataFlash.Log_Write_IMU(ins);
DataFlash.Log_Write_IMUDT(ins);
}
}
// get_closest_point_on_circle - returns closest point on the circle
// circle's center should already have been set
// closest point on the circle will be placed in result
// result's altitude (i.e. z) will be set to the circle_center's altitude
// if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
void AC_Circle::get_closest_point_on_circle(Vector3f &result)
{
// return center if radius is zero
if (_radius <= 0) {
result = _center;
return;
}
// get current position
const Vector3f &curr_pos = _inav.get_position();
// calc vector from current location to circle center
Vector2f vec; // vector from circle center to current location
vec.x = (curr_pos.x - _center.x);
vec.y = (curr_pos.y - _center.y);
float dist = pythagorous2(vec.x, vec.y);
// if current location is exactly at the center of the circle return edge directly behind vehicle
if (is_zero(dist)) {
result.x = _center.x - _radius * _ahrs.cos_yaw();
result.y = _center.y - _radius * _ahrs.sin_yaw();
result.z = _center.z;
return;
}
// calculate closest point on edge of circle
result.x = _center.x + vec.x / dist * _radius;
result.y = _center.y + vec.y / dist * _radius;
result.z = _center.z;
}
/// rate_to_accel_xy - horizontal desired rate to desired acceleration
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void AC_PosControl::rate_to_accel_xy(float dt)
{
const Vector3f &vel_curr = _inav.get_velocity(); // current velocity in cm/s
float accel_total; // total acceleration in cm/s/s
float lat_i, lon_i;
// reset last velocity target to current target
if (_flags.reset_rate_to_accel_xy) {
_vel_last.x = _vel_target.x;
_vel_last.y = _vel_target.y;
_flags.reset_rate_to_accel_xy = false;
}
// feed forward desired acceleration calculation
if (dt > 0.0f) {
_accel_target.x = (_vel_target.x - _vel_last.x)/dt;
_accel_target.y = (_vel_target.y - _vel_last.y)/dt;
} else {
_accel_target.x = 0.0f;
_accel_target.y = 0.0f;
}
// store this iteration's velocities for the next iteration
_vel_last.x = _vel_target.x;
_vel_last.y = _vel_target.y;
// calculate velocity error
_vel_error.x = _vel_target.x - vel_curr.x;
_vel_error.y = _vel_target.y - vel_curr.y;
// get current i term
lat_i = _pid_rate_lat.get_integrator();
lon_i = _pid_rate_lon.get_integrator();
// update i term if we have not hit the accel or throttle limits OR the i term will reduce
if ((!_limit.accel_xy && !_motors.limit.throttle_upper) || ((lat_i>0&&_vel_error.x<0)||(lat_i<0&&_vel_error.x>0))) {
lat_i = _pid_rate_lat.get_i(_vel_error.x, dt);
}
if ((!_limit.accel_xy && !_motors.limit.throttle_upper) || ((lon_i>0&&_vel_error.y<0)||(lon_i<0&&_vel_error.y>0))) {
lon_i = _pid_rate_lon.get_i(_vel_error.y, dt);
}
// combine feed forward accel with PID output from velocity error
_accel_target.x += _pid_rate_lat.get_p(_vel_error.x) + lat_i + _pid_rate_lat.get_d(_vel_error.x, dt);
_accel_target.y += _pid_rate_lon.get_p(_vel_error.y) + lon_i + _pid_rate_lon.get_d(_vel_error.y, dt);
// scale desired acceleration if it's beyond acceptable limit
// To-Do: move this check down to the accel_to_lean_angle method?
accel_total = pythagorous2(_accel_target.x, _accel_target.y);
if (accel_total > POSCONTROL_ACCEL_XY_MAX && accel_total > 0.0f) {
_accel_target.x = POSCONTROL_ACCEL_XY_MAX * _accel_target.x/accel_total;
_accel_target.y = POSCONTROL_ACCEL_XY_MAX * _accel_target.y/accel_total;
_limit.accel_xy = true; // unused
} else {
// reset accel limit flag
_limit.accel_xy = false;
}
}
/// accel_to_lean_angles - horizontal desired acceleration to lean angles
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bool use_althold_lean_angle)
{
float accel_total; // total acceleration in cm/s/s
float accel_right, accel_forward;
float lean_angle_max = _attitude_control.lean_angle_max();
float accel_max = POSCONTROL_ACCEL_XY_MAX;
// limit acceleration if necessary
if (use_althold_lean_angle) {
accel_max = min(accel_max, GRAVITY_MSS * 100.0f * sinf(ToRad(constrain_float(_attitude_control.get_althold_lean_angle_max(),1000,8000)/100.0f)));
}
// scale desired acceleration if it's beyond acceptable limit
accel_total = pythagorous2(_accel_target.x, _accel_target.y);
if (accel_total > accel_max && accel_total > 0.0f) {
_accel_target.x = accel_max * _accel_target.x/accel_total;
_accel_target.y = accel_max * _accel_target.y/accel_total;
_limit.accel_xy = true; // unused
} else {
// reset accel limit flag
_limit.accel_xy = false;
}
// reset accel to current desired acceleration
if (_flags.reset_accel_to_lean_xy) {
_accel_target_jerk_limited.x = _accel_target.x;
_accel_target_jerk_limited.y = _accel_target.y;
_accel_target_filter.reset(Vector2f(_accel_target.x, _accel_target.y));
_flags.reset_accel_to_lean_xy = false;
}
// apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
float max_delta_accel = dt * _jerk_cmsss;
Vector2f accel_in(_accel_target.x, _accel_target.y);
Vector2f accel_change = accel_in-_accel_target_jerk_limited;
float accel_change_length = accel_change.length();
if(accel_change_length > max_delta_accel) {
accel_change *= max_delta_accel/accel_change_length;
}
_accel_target_jerk_limited += accel_change;
// lowpass filter on NE accel
_accel_target_filter.set_cutoff_frequency(min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt);
// rotate accelerations into body forward-right frame
accel_forward = accel_target_filtered.x*_ahrs.cos_yaw() + accel_target_filtered.y*_ahrs.sin_yaw();
accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw();
// update angle targets that will be passed to stabilize controller
_pitch_target = constrain_float(atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI_F),-lean_angle_max, lean_angle_max);
float cos_pitch_target = cosf(_pitch_target*M_PI_F/18000);
_roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI_F), -lean_angle_max, lean_angle_max);
}
// _yaw_gain reduces the gain of the PI controller applied to heading errors
// when observability from change of velocity is good (eg changing speed or turning)
// This reduces unwanted roll and pitch coupling due to compass errors for planes.
// High levels of noise on _accel_ef will cause the gain to drop and could lead to
// increased heading drift during straight and level flight, however some gain is
// always available. TODO check the necessity of adding adjustable acc threshold
// and/or filtering accelerations before getting magnitude
float
AP_AHRS_DCM::_yaw_gain(void) const
{
float VdotEFmag = pythagorous2(_accel_ef[_active_accel_instance].x,
_accel_ef[_active_accel_instance].y);
if (VdotEFmag <= 4.0f) {
return 0.2f*(4.5f - VdotEFmag);
}
return 0.1f;
}
float Copter::get_look_ahead_yaw()
{
const Vector3f& vel = inertial_nav.get_velocity();
float speed = pythagorous2(vel.x,vel.y);
// Commanded Yaw to automatically look ahead.
if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f;
}
return yaw_look_ahead_bearing;
}
/*
set and save accelerometer bias along with trim calculation
*/
void AP_InertialSensor::_acal_save_calibrations()
{
Vector3f bias, gain;
for (uint8_t i=0; i<_accel_count; i++) {
if (_accel_calibrator[i].get_status() == ACCEL_CAL_SUCCESS) {
_accel_calibrator[i].get_calibration(bias, gain);
_accel_offset[i].set_and_save(bias);
_accel_scale[i].set_and_save(gain);
} else {
_accel_offset[i].set_and_save(Vector3f(0,0,0));
_accel_scale[i].set_and_save(Vector3f(0,0,0));
}
}
Vector3f aligned_sample;
Vector3f misaligned_sample;
switch(_trim_option) {
case 0:
break;
case 1:
// The first level step of accel cal will be taken as gnd truth,
// i.e. trim will be set as per the output of primary accel from the level step
get_primary_accel_cal_sample_avg(0,aligned_sample);
_trim_pitch = atan2f(aligned_sample.x, pythagorous2(aligned_sample.y, aligned_sample.z));
_trim_roll = atan2f(-aligned_sample.y, -aligned_sample.z);
_new_trim = true;
break;
case 2:
// Reference accel is truth, in this scenario there is a reference accel
// as mentioned in ACC_BODY_ALIGNED
if (get_primary_accel_cal_sample_avg(0,misaligned_sample) && get_fixed_mount_accel_cal_sample(0,aligned_sample)) {
// determine trim from aligned sample vs misaligned sample
Vector3f cross = (misaligned_sample%aligned_sample);
float dot = (misaligned_sample*aligned_sample);
Quaternion q(safe_sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z);
q.normalize();
_trim_roll = q.get_euler_roll();
_trim_pitch = q.get_euler_pitch();
_new_trim = true;
}
break;
default:
_new_trim = false;
/* no break */
}
if (fabsf(_trim_roll) > radians(10) ||
fabsf(_trim_pitch) > radians(10)) {
hal.console->print("ERR: Trim over maximum of 10 degrees!!");
_new_trim = false; //we have either got faulty level during acal or highly misaligned accelerometers
}
_accel_cal_requires_reboot = true;
}
void
AP_Mount::calc_GPS_target_angle(const struct Location *target)
{
float GPS_vector_x = (target->lng-_current_loc->lng)*cosf(ToRad((_current_loc->lat+target->lat)*0.00000005f))*0.01113195f;
float GPS_vector_y = (target->lat-_current_loc->lat)*0.01113195f;
float GPS_vector_z = (target->alt-_current_loc->alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
_roll_control_angle = 0;
_tilt_control_angle = atan2f(GPS_vector_z, target_distance);
_pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y);
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has performed all required GPS_ok checks
void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m)
{
// convert location to vector from ekf origin
Vector3f circle_center_neu;
if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) {
// default to current position and log error
circle_center_neu = inertial_nav.get_position();
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
}
circle_nav.set_center(circle_center_neu);
// set circle radius
if (!is_zero(radius_m)) {
circle_nav.set_radius(radius_m * 100.0f);
}
// check our distance from edge of circle
Vector3f circle_edge_neu;
circle_nav.get_closest_point_on_circle(circle_edge_neu);
float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
// if more than 3m then fly to edge
if (dist_to_edge > 300.0f) {
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// convert circle_edge_neu to Location_Class
Location_Class circle_edge(circle_edge_neu);
// convert altitude to same as command
circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
// initialise wpnav to move to edge of circle
if (!wp_nav.set_wp_destination(circle_edge)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
}
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = pythagorous2(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
} else {
auto_circle_start();
}
}
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
{
// range check nav_dt
if( nav_dt < 0 ) {
return;
}
// check loiter speed and avoid divide by zero
if( _loiter_speed_cms < 100.0f) {
_loiter_speed_cms = 100.0f;
_loiter_accel_cms = _loiter_speed_cms/2.0f;
}
// rotate pilot input to lat/lon frame
Vector2f desired_accel;
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());
// get pos_control's feed forward velocity
Vector2f desired_vel = _pos_control.get_desired_velocity();
// add pilot commanded acceleration
desired_vel += desired_accel * nav_dt;
// reduce velocity with fake wind resistance
if(desired_vel.x > 0 ) {
desired_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.x/_loiter_speed_cms;
desired_vel.x = max(desired_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}else if(desired_vel.x < 0) {
desired_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.x/_loiter_speed_cms;
desired_vel.x = min(desired_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}
if(desired_vel.y > 0 ) {
desired_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.y/_loiter_speed_cms;
desired_vel.y = max(desired_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}else if(desired_vel.y < 0) {
desired_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.y/_loiter_speed_cms;
desired_vel.y = min(desired_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}
// constrain and scale the feed forward velocity if necessary
float vel_total = pythagorous2(desired_vel.x, desired_vel.y);
if (vel_total > _loiter_speed_cms && vel_total > 0.0f) {
desired_vel.x = _loiter_speed_cms * desired_vel.x/vel_total;
desired_vel.y = _loiter_speed_cms * desired_vel.y/vel_total;
}
// send adjusted feed forward velocity back to position controller
_pos_control.set_desired_velocity(desired_vel.x,desired_vel.y);
}
/*
_calculate_trim - calculates the x and y trim angles. The
accel_sample must be correctly scaled, offset and oriented for the
board
*/
bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch)
{
trim_pitch = atan2f(accel_sample.x, pythagorous2(accel_sample.y, accel_sample.z));
trim_roll = atan2f(-accel_sample.y, -accel_sample.z);
if (fabsf(trim_roll) > radians(10) ||
fabsf(trim_pitch) > radians(10)) {
hal.console->println("trim over maximum of 10 degrees");
return false;
}
hal.console->printf("Trim OK: roll=%.2f pitch=%.2f\n",
(double)degrees(trim_roll),
(double)degrees(trim_pitch));
return true;
}
/*
* reset the DCM matrix and omega. Used on ground start, and on
* extreme errors in the matrix
*/
void
AP_AHRS_DCM::reset(bool recover_eulers)
{
// reset the integration terms
_omega_I.zero();
_omega_P.zero();
_omega_yaw_P.zero();
_omega.zero();
// if the caller wants us to try to recover to the current
// attitude then calculate the dcm matrix from the current
// roll/pitch/yaw values
if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
_dcm_matrix.from_euler(roll, pitch, yaw);
} else {
// Use the measured accel due to gravity to calculate an initial
// roll and pitch estimate
// Get body frame accel vector
Vector3f initAccVec;
uint8_t counter = 0;
initAccVec = _ins.get_accel();
// the first vector may be invalid as the filter starts up
while (initAccVec.length() <= 5.0f && counter++ < 10) {
_ins.wait_for_sample();
_ins.update();
initAccVec = _ins.get_accel();
}
// normalise the acceleration vector
if (initAccVec.length() > 5.0f) {
// calculate initial pitch angle
pitch = atan2f(initAccVec.x, pythagorous2(initAccVec.y, initAccVec.z));
// calculate initial roll angle
roll = atan2f(-initAccVec.y, -initAccVec.z);
} else {
// If we cant use the accel vector, then align flat
roll = 0.0f;
pitch = 0.0f;
}
_dcm_matrix.from_euler(roll, pitch, 0);
}
_last_startup_ms = AP_HAL::millis();
}
void Rover::update_GPS_10Hz(void)
{
have_position = ahrs.get_position(current_loc);
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_gps_msg_ms = gps.last_message_time_ms();
if (ground_start_count > 1){
ground_start_count--;
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
ground_start_count = 20;
} else {
init_home();
// set system clock for log timestamps
hal.util->set_system_clock(gps.time_epoch_usec());
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(gps.location().lat, gps.location().lng);
}
ground_start_count = 0;
}
}
Vector3f velocity;
if (ahrs.get_velocity_NED(velocity)) {
ground_speed = pythagorous2(velocity.x, velocity.y);
} else {
ground_speed = gps.ground_speed();
}
#if CAMERA == ENABLED
if (camera.update_location(current_loc, rover.ahrs) == true) {
do_take_picture();
}
#endif
if (!hal.util->get_soft_armed()) {
update_home();
}
}
}
// crash_check - disarms motors if a crash has been detected
// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
// called at MAIN_LOOP_RATE
void Copter::crash_check()
{
static uint16_t crash_counter; // number of iterations vehicle may have been crashed
// return immediately if disarmed, or crash checking disabled
if (!motors.armed() || ap.land_complete || g.fs_crash_check == 0) {
crash_counter = 0;
return;
}
// return immediately if we are not in an angle stabilize flight mode or we are flipping
if (control_mode == ACRO || control_mode == FLIP) {
crash_counter = 0;
return;
}
// vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
if (land_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
crash_counter = 0;
return;
}
// check for angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) {
crash_counter = 0;
return;
}
// we may be crashing
crash_counter++;
// check if crashing for 2 seconds
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * MAIN_LOOP_RATE)) {
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// send message to gcs
gcs_send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
// disarm motors
init_disarm_motors();
hal.uartA->printf("CRASH!");
}
}
/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
/// distance_max allows limiting distance to stopping point
/// results placed in stopping_position vector
/// set_accel_xy() should be called before this method to set vehicle acceleration
/// set_leash_length() should have been called before this method
void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
{
const Vector3f curr_pos = _inav.get_position();
Vector3f curr_vel = _inav.get_velocity();
float linear_distance; // the distance at which we swap from a linear to sqrt response
float linear_velocity; // the velocity above which we swap from a linear to sqrt response
float stopping_dist; // the distance within the vehicle can stop
float kP = _p_pos_xy.kP();
// add velocity error to current velocity
if (is_active_xy()) {
curr_vel.x += _vel_error.x;
curr_vel.y += _vel_error.y;
}
// calculate current velocity
float vel_total = pythagorous2(curr_vel.x, curr_vel.y);
// avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
if (kP <= 0.0f || _accel_cms <= 0.0f || vel_total == 0.0f) {
stopping_point.x = curr_pos.x;
stopping_point.y = curr_pos.y;
return;
}
// calculate point at which velocity switches from linear to sqrt
linear_velocity = _accel_cms/kP;
// calculate distance within which we can stop
if (vel_total < linear_velocity) {
stopping_dist = vel_total/kP;
} else {
linear_distance = _accel_cms/(2.0f*kP*kP);
stopping_dist = linear_distance + (vel_total*vel_total)/(2.0f*_accel_cms);
}
// constrain stopping distance
stopping_dist = constrain_float(stopping_dist, 0, _leash);
// convert the stopping distance into a stopping point using velocity vector
stopping_point.x = curr_pos.x + (stopping_dist * curr_vel.x / vel_total);
stopping_point.y = curr_pos.y + (stopping_dist * curr_vel.y / vel_total);
}
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
void AC_WPNav::calculate_wp_leash_length()
{
// length of the unit direction vector in the horizontal
float pos_delta_unit_xy = pythagorous2(_pos_delta_unit.x, _pos_delta_unit.y);
float pos_delta_unit_z = fabsf(_pos_delta_unit.z);
float speed_z;
float leash_z;
if (_pos_delta_unit.z >= 0.0f) {
speed_z = _wp_speed_up_cms;
leash_z = _pos_control.get_leash_up_z();
}else{
speed_z = _wp_speed_down_cms;
leash_z = _pos_control.get_leash_down_z();
}
// calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel
if(is_zero(pos_delta_unit_z) && is_zero(pos_delta_unit_xy)){
_track_accel = 0;
_track_speed = 0;
_track_leash_length = WPNAV_LEASH_LENGTH_MIN;
}else if(is_zero(_pos_delta_unit.z)){
_track_accel = _wp_accel_cms/pos_delta_unit_xy;
_track_speed = _wp_speed_cms/pos_delta_unit_xy;
_track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy;
}else if(is_zero(pos_delta_unit_xy)){
_track_accel = _wp_accel_z_cms/pos_delta_unit_z;
_track_speed = speed_z/pos_delta_unit_z;
_track_leash_length = leash_z/pos_delta_unit_z;
}else{
_track_accel = MIN(_wp_accel_z_cms/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy);
_track_speed = MIN(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);
_track_leash_length = MIN(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy);
}
// calculate slow down distance (the distance from the destination when the target point should begin to slow down)
calc_slow_down_distance(_track_speed, _track_accel);
// set recalc leash flag to false
_flags.recalc_wp_leash = false;
}
/// desired_vel_to_pos - move position target using desired velocities
void AC_PosControl::desired_vel_to_pos(float nav_dt)
{
Vector2f target_vel_adj;
float vel_desired_total;
// range check nav_dt
if( nav_dt < 0 ) {
return;
}
// constrain and scale the desired velocity
vel_desired_total = pythagorous2(_vel_desired.x, _vel_desired.y);
if (vel_desired_total > _speed_cms && vel_desired_total > 0.0f) {
_vel_desired.x = _speed_cms * _vel_desired.x/vel_desired_total;
_vel_desired.y = _speed_cms * _vel_desired.y/vel_desired_total;
}
// update target position
_pos_target.x += _vel_desired.x * nav_dt;
_pos_target.y += _vel_desired.y * nav_dt;
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
void Copter::auto_circle_movetoedge_start()
{
// check our distance from edge of circle
Vector3f circle_edge;
circle_nav.get_closest_point_on_circle(circle_edge);
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// initialise wpnav to move to edge of circle
wp_nav.set_wp_destination(circle_edge);
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
const Vector3f &circle_center = circle_nav.get_center();
float dist_to_center = pythagorous2(circle_center.x - curr_pos.x, circle_center.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// decay GPS horizontal position offset to close to zero at a rate of 1 m/s for copters and 5 m/s for planes
// limit radius to a maximum of 50m
void NavEKF2_core::decayGpsOffset()
{
float offsetDecaySpd;
if (assume_zero_sideslip()) {
offsetDecaySpd = 5.0f;
} else {
offsetDecaySpd = 1.0f;
}
float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms);
lastDecayTime_ms = imuSampleTime_ms;
float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y);
// decay radius if larger than offset decay speed multiplied by lapsed time (plus a margin to prevent divide by zero)
if (offsetRadius > (offsetDecaySpd * lapsedTime + 0.1f)) {
// Calculate the GPS velocity offset required. This is necessary to prevent the position measurement being rejected for inconsistency when the radius is being pulled back in.
gpsVelGlitchOffset = -gpsPosGlitchOffsetNE*offsetDecaySpd/offsetRadius;
// calculate scale factor to be applied to both offset components
float scaleFactor = constrain_float((offsetRadius - offsetDecaySpd * lapsedTime), 0.0f, 50.0f) / offsetRadius;
gpsPosGlitchOffsetNE.x *= scaleFactor;
gpsPosGlitchOffsetNE.y *= scaleFactor;
} else {
gpsVelGlitchOffset.zero();
gpsPosGlitchOffsetNE.zero();
}
}
/* Monitor GPS data to see if quality is good enough to initialise the EKF
Monitor magnetometer innovations to to see if the heading is good enough to use GPS
Return true if all criteria pass for 10 seconds
We also record the failure reason so that prearm_failure_reason()
can give a good report to the user on why arming is failing
*/
bool NavEKF2_core::calcGpsGoodToAlign(void)
{
// fail if reported speed accuracy greater than threshold
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f) && (frontend._gpsCheck & MASK_GPS_SPD_ERR);
// Report check result as a text string and bitmask
if (gpsSpdAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS speed error %.1f", (double)gpsSpdAccuracy);
gpsCheckStatus.bad_sAcc = true;
} else {
gpsCheckStatus.bad_sAcc = false;
}
// fail if not enough sats
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend._gpsCheck & MASK_GPS_NSATS);
// Report check result as a text string and bitmask
if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
gpsCheckStatus.bad_sats = true;
} else {
gpsCheckStatus.bad_sats = false;
}
// fail if satellite geometry is poor
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend._gpsCheck & MASK_GPS_HDOP);
// Report check result as a text string and bitmask
if (hdopFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop()));
gpsCheckStatus.bad_hdop = true;
} else {
gpsCheckStatus.bad_hdop = false;
}
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
bool hAccFail;
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
hAccFail = (hAcc > 5.0f) && (frontend._gpsCheck & MASK_GPS_POS_ERR);
} else {
hAccFail = false;
}
// Report check result as a text string and bitmask
if (hAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horiz error %.1f", (double)hAcc);
gpsCheckStatus.bad_hAcc = true;
} else {
gpsCheckStatus.bad_hAcc = false;
}
// If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
// This enables us to handle large changes to the external magnetic field environment that occur before arming
if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f) || !consistentMagData) {
magYawResetTimer_ms = imuSampleTime_ms;
}
if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
// reset heading and field states
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
// reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
magYawResetTimer_ms = imuSampleTime_ms;
}
// fail if magnetometer innovations are outside limits indicating bad yaw
// with bad yaw we are unable to use GPS
bool yawFail;
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (frontend._gpsCheck & MASK_GPS_YAW_ERR)) {
yawFail = true;
} else {
yawFail = false;
}
// Report check result as a text string and bitmask
if (yawFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"Mag yaw error x=%.1f y=%.1f",
(double)magTestRatio.x,
(double)magTestRatio.y);
gpsCheckStatus.bad_yaw = true;
} else {
gpsCheckStatus.bad_yaw = false;
}
// Check for significant change in GPS position if disarmed which indicates bad GPS
// This check can only be used when the vehicle is stationary
const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
// calculate time lapsesd since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved
gpsDriftNE += location_diff(gpsloc_prev, gpsloc).length();
gpsloc_prev = gpsloc;
// Decay distance moved exponentially to zero
gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst);
// Clamp the fiter state to prevent excessive persistence of large transients
gpsDriftNE = min(gpsDriftNE,10.0f);
// Fail if more than 3 metres drift after filtering whilst on-ground
// This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m
bool gpsDriftFail = (gpsDriftNE > 3.0f) && onGround && (frontend._gpsCheck & MASK_GPS_POS_DRIFT);
// Report check result as a text string and bitmask
if (gpsDriftFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS drift %.1fm (needs 3.0)", (double)gpsDriftNE);
gpsCheckStatus.bad_horiz_drift = true;
} else {
gpsCheckStatus.bad_horiz_drift = false;
}
// Check that the vertical GPS vertical velocity is reasonable after noise filtering
bool gpsVertVelFail;
if (_ahrs->get_gps().have_vertical_velocity() && onGround) {
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f) && (frontend._gpsCheck & MASK_GPS_VERT_SPD);
} else if ((frontend._fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
} else {
gpsVertVelFail = false;
}
// Report check result as a text string and bitmask
if (gpsVertVelFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS vertical speed %.2fm/s (needs 0.30)", (double)fabsf(gpsVertVelFilt));
gpsCheckStatus.bad_vert_vel = true;
} else {
gpsCheckStatus.bad_vert_vel = false;
}
// Check that the horizontal GPS vertical velocity is reasonable after noise filtering
// This check can only be used if the vehicle is stationary
bool gpsHorizVelFail;
if (onGround) {
gpsHorizVelFilt = 0.1f * pythagorous2(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f) && (frontend._gpsCheck & MASK_GPS_HORIZ_SPD);
} else {
gpsHorizVelFail = false;
}
// Report check result as a text string and bitmask
if (gpsHorizVelFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horizontal speed %.2fm/s (needs 0.30)", (double)gpsDriftNE);
gpsCheckStatus.bad_horiz_vel = true;
} else {
gpsCheckStatus.bad_horiz_vel = false;
}
// record time of fail - assume fail first time called
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail || lastGpsVelFail_ms == 0) {
lastGpsVelFail_ms = imuSampleTime_ms;
}
// continuous period without fail required to return a healthy status
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
return true;
} else {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF checking GPS");
return false;
}
}
// set_velocity_xy - set velocity in latitude & longitude directions (in cm/s)
float AP_InertialNav::get_velocity_xy() const
{
return pythagorous2(_velocity.x, _velocity.y);
}
/// get_distance_to_destination - get horizontal distance to destination in cm
float AC_WPNav::get_distance_to_destination()
{
// get current location
Vector3f curr = _inav->get_position();
return pythagorous2(_destination.x-curr.x,_destination.y-curr.y);
}
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// returns desired angle rates in centi-degrees-per-second
void Sub::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
{
float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
// apply circular limit to pitch and roll inputs
float total_in = pythagorous2((float)pitch_in, (float)roll_in);
if (total_in > ROLL_PITCH_INPUT_MAX) {
float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// calculate roll, pitch rate requests
if (g.acro_expo <= 0) {
rate_bf_request.x = roll_in * g.acro_rp_p;
rate_bf_request.y = pitch_in * g.acro_rp_p;
} else {
// expo variables
float rp_in, rp_in3, rp_out;
// range check expo
if (g.acro_expo > 1.0f) {
g.acro_expo = 1.0f;
}
// roll expo
rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
// pitch expo
rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
}
// calculate yaw rate request
rate_bf_request.z = yaw_in * g.acro_yaw_p;
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
// Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
// Calculate trainer mode earth frame rate command for yaw
rate_ef_level.z = 0;
// Calculate angle limiting earth frame rate commands
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > aparm.angle_max){
rate_ef_level.x -= g.acro_balance_roll*(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) {
rate_ef_level.x -= g.acro_balance_roll*(roll_angle+aparm.angle_max);
}
if (pitch_angle > aparm.angle_max){
rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-aparm.angle_max);
}else if (pitch_angle < -aparm.angle_max) {
rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+aparm.angle_max);
}
}
// convert earth-frame level rates to body-frame level rates
attitude_control.euler_rate_to_ang_vel(attitude_control.get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);
// combine earth frame rate corrections with rate requests
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z;
}else{
float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
// Scale leveling rates by stick input
rate_bf_level = rate_bf_level*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x));
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y));
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z));
rate_bf_request.z += rate_bf_level.z;
rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
}
}
// hand back rate request
roll_out = rate_bf_request.x;
pitch_out = rate_bf_request.y;
yaw_out = rate_bf_request.z;
}
/// pos_to_rate_xy - horizontal position error to velocity controller
/// converts position (_pos_target) to target velocity (_vel_target)
/// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s
void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt)
{
Vector3f curr_pos = _inav.get_position();
float linear_distance; // the distance we swap between linear and sqrt velocity response
float kP = _p_pos_xy.kP();
// avoid divide by zero
if (kP <= 0.0f) {
_vel_target.x = 0.0f;
_vel_target.y = 0.0f;
}else{
// calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
_pos_error.y = _pos_target.y - curr_pos.y;
// constrain target position to within reasonable distance of current location
_distance_to_target = pythagorous2(_pos_error.x, _pos_error.y);
if (_distance_to_target > _leash && _distance_to_target > 0.0f) {
_pos_target.x = curr_pos.x + _leash * _pos_error.x/_distance_to_target;
_pos_target.y = curr_pos.y + _leash * _pos_error.y/_distance_to_target;
// re-calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
_pos_error.y = _pos_target.y - curr_pos.y;
_distance_to_target = _leash;
}
// calculate the distance at which we swap between linear and sqrt velocity response
linear_distance = _accel_cms/(2.0f*kP*kP);
if (_distance_to_target > 2.0f*linear_distance) {
// velocity response grows with the square root of the distance
float vel_sqrt = safe_sqrt(2.0f*_accel_cms*(_distance_to_target-linear_distance));
_vel_target.x = vel_sqrt * _pos_error.x/_distance_to_target;
_vel_target.y = vel_sqrt * _pos_error.y/_distance_to_target;
}else{
// velocity response grows linearly with the distance
_vel_target.x = _p_pos_xy.kP() * _pos_error.x;
_vel_target.y = _p_pos_xy.kP() * _pos_error.y;
}
// decide velocity limit due to position error
float vel_max_from_pos_error;
if (use_desired_rate) {
// if desired velocity (i.e. velocity feed forward) is being used we limit the maximum velocity correction due to position error to 2m/s
vel_max_from_pos_error = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR;
}else{
// if desired velocity is not used, we allow position error to increase speed up to maximum speed
vel_max_from_pos_error = _speed_cms;
}
// scale velocity to stays within limits
float vel_total = pythagorous2(_vel_target.x, _vel_target.y);
if (vel_total > vel_max_from_pos_error) {
_vel_target.x = vel_max_from_pos_error * _vel_target.x/vel_total;
_vel_target.y = vel_max_from_pos_error * _vel_target.y/vel_total;
}
// add desired velocity (i.e. feed forward).
if (use_desired_rate) {
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
}
}
}