/// 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 * tanf(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 = norm(_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),-lean_angle_max, lean_angle_max);
float cos_pitch_target = cosf(_pitch_target*M_PI/18000);
_roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
}
/// 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)
{
float accel_right, accel_forward;
float lean_angle_max = _attitude_control.lean_angle_max();
// 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_filtered.x = _accel_target.x;
_accel_target_filtered.y = _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 * POSCONTROL_JERK_LIMIT_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;
// 5Hz lowpass filter on NE accel
float freq_cut = POSCONTROL_ACCEL_FILTER_HZ * ekfNavVelGainScaler;
float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI*freq_cut)),0.0f,1.0f);
_accel_target_filtered.x += alpha * (_accel_target_jerk_limited.x - _accel_target_filtered.x);
_accel_target_filtered.y += alpha * (_accel_target_jerk_limited.y - _accel_target_filtered.y);
// 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(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);
float cos_pitch_target = cosf(_pitch_target*(float)M_PI/18000);
_roll_target = constrain_float(fast_atan(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
}
/// 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)
{
float accel_right, accel_forward;
float lean_angle_max = _attitude_control.lean_angle_max();
// 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 * POSCONTROL_JERK_LIMIT_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);
}
// Just some addition of use Forward Thruster when certain amount of accel_forward is requested.
void AC_PosControl_Compound::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 * tanf(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 = norm(_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
// if much accel_forward is requested use forward thruster.
_pitch_target = constrain_float(atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);
float cos_pitch_target = cosf(_pitch_target*M_PI/18000);
_roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
//To-Do : Do not allow use thruster which requires level of precision like circle, land modes, ...
// To-Do 2 : Possibly There would be optimal ways to mix pitch down and rear throttle controller.
// if (_use_thruster)
// {
// if (accel_forward >= 0.0f)
// {
// // only allow small amount of nose down
// // _pitch_target = constrain_float(_pitch_target,-500,0);
// // _pitch_target = 0.0f;
//
// // run_auxiliary_thruster_controller(accel_forward);
// }
// else
// {
// run_auxiliary_thruster_controller(0.0f);
// _motors.set_forward(0.0f);
// }
// }
}