/*
float vector_magnitude(float quadrature_component, float direct_component)
{
float magnitude;
magnitude=sqrtf( (quadrature_component*quadrature_component+direct_component*direct_component) );
return magnitude;
}
float vector_angle(float quadrature_component, float direct_component)
{
float angle;
//angle=180.0f/PI*atanf(quadrature_component/direct_component);
angle=57.295779513082320f*atanf(quadrature_component/direct_component);
if (angle>=360.0f)
angle=angle-360.0f;
else if (angle<0.0f)
angle=angle+360.0f;
if ( (quadrature_component<=0.0f) && (direct_component<0.0f) )
angle=angle+180.0f;
else if ( (quadrature_component>0.0f)&&(direct_component<0.0f))
angle=angle-180.0f;
return angle;
}
*/
float fast_vector_angle(float y, float x)
{
float angle;
/*
if (x>=0.0f && y>=0.0f && x>= y) { angle = fast_atan( y/ x); } //1st quadrant
else if (x>=0.0f && y>=0.0f && y> x) { angle = 90.0f-fast_atan( x/ y); } //2nd quadrant
else if (x< 0.0f && y>=0.0f && y>=-x) { angle = 90.0f+fast_atan(-x/ y); } //3rd quadrant
else if (x< 0.0f && y>=0.0f && -x> y) { angle = 180.0f-fast_atan( y/-x); } //4th quadrant
else if (x< 0.0f && y< 0.0f && -x>=-y) { angle = 180.0f+fast_atan(-y/-x); } //5th quadrant
else if (x< 0.0f && y< 0.0f && -y>-x) { angle = 270.0f-fast_atan(-x/-y); } //6th quadrant
else if (x>=0.0f && y< 0.0f && -y>= x) { angle = 270.0f+fast_atan( x/-y); } //7th quadrant
else if (x>=0.0f && y< 0.0f && x>-y) { angle = 360.0f-fast_atan(-y/ x); } //8th quadrant
else { angle = 0.0f ; } //lost quadrant
*/
if (x>=0.0f && y>=0.0f && x>= y) { angle = fast_atan( y/ x); } //1st quadrant
else if (x>=0.0f && y>=0.0f && y> x) { angle = 90.0f-fast_atan( x/ y); } //2nd quadrant
else if (x< 0.0f && y>=0.0f && y>=-x) { angle = 90.0f+fast_atan(-x/ y); } //3rd quadrant
else if (x< 0.0f && y>=0.0f && -x> y) { angle = 180.0f-fast_atan( y/-x); } //4th quadrant
else if (x< 0.0f && y< 0.0f && -x>=-y) { angle = 180.0f+fast_atan(-y/-x); } //5th quadrant
else if (x< 0.0f && y< 0.0f && -y>-x) { angle = 270.0f-fast_atan(-x/-y); } //6th quadrant
else if (x>=0.0f && y< 0.0f && -y>= x) { angle = 270.0f+fast_atan( x/-y); } //7th quadrant
else if (x>=0.0f && y< 0.0f && x>-y) { angle = 360.0f-fast_atan(-y/ x); } //8th quadrant
else { angle = 0.0f ; }
return angle;
}
/// 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 accel_right, accel_forward;
float lean_angle_max = _attitude_control.lean_angle_max();
// To-Do: add 1hz filter to accel_lat, accel_lon
// rotate accelerations into body forward-right frame
accel_forward = _accel_target.x*_ahrs.cos_yaw() + _accel_target.y*_ahrs.sin_yaw();
accel_right = -_accel_target.x*_ahrs.sin_yaw() + _accel_target.y*_ahrs.cos_yaw();
// update angle targets that will be passed to stabilize controller
_roll_target = constrain_float(fast_atan(accel_right*_ahrs.cos_pitch()/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
_pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);
}
/// get_loiter_acceleration_to_lean_angles - loiter acceleration controller
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float accel_lon)
{
float z_accel_meas = -GRAVITY_MSS * 100; // gravity in cm/s/s
float accel_forward;
float accel_right;
// To-Do: add 1hz filter to accel_lat, accel_lon
// rotate accelerations into body forward-right frame
accel_forward = accel_lat*_cos_yaw + accel_lon*_sin_yaw;
accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
// update angle targets that will be passed to stabilize controller
_desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -MAX_LEAN_ANGLE, MAX_LEAN_ANGLE);
_desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI), -MAX_LEAN_ANGLE, MAX_LEAN_ANGLE);
}
/// 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);
}