// get_bearing_cd - return bearing in centi-degrees between two positions
// To-Do: move this to math library
float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const
{
float bearing = 9000 + fast_atan2(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;
if (bearing < 0) {
bearing += 36000;
}
return bearing;
}
/// advance_spline_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);
// update velocity
float spline_dist_to_wp = (_destination - target_pos).length();
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms);
} else if(_spline_vel_scaler < _wp_speed_cms) {
// increase velocity using acceleration
// To-Do: replace 0.1f below with update frequency passed in from main program
_spline_vel_scaler += _wp_accel_cms* 0.1f;
}
// constrain target velocity
if (_spline_vel_scaler > _wp_speed_cms) {
_spline_vel_scaler = _wp_speed_cms;
}
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
float target_vel_length = target_vel.length();
if (target_vel_length != 0.0f) {
_spline_time_scale = _spline_vel_scaler/target_vel_length;
}
// update target position
_pos_control.set_pos_target(target_pos);
// update the yaw
_yaw = RadiansToCentiDegrees(fast_atan2(target_vel.y,target_vel.x));
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
// we will reach the next waypoint in the next step so set reached_destination flag
// To-Do: is this one step too early?
if (_spline_time >= 1.0f) {
_flags.reached_destination = true;
}
}
}
void fastRotationEstimation(const unsigned char *image, IPoint *ip)
{
int i;
double dx = 0.0;
double dy = 0.0;
const int pos = ip->pos;
const double centrepx = (double)image[pos];
const int stride = pyr_img1_width >> ip->pyrLevel;
const double *ring_x = fast_ring_x;
const double *ring_y = fast_ring_y;
for (i = 0; i < 16; i++)
{
const double diff = (double)image[ (indY[i] * stride + indX[i]) + pos ] - centrepx;
dx += diff * (*(ring_x++));
dy += diff * (*(ring_y++));
}
ip->orientation = getCoterminalAngle( fast_atan2(dy, dx) );//ANGLE(dx, dy);
}
static void
pll (AMD am, COMPLEX sig)
{
COMPLEX z = Cmplx ((REAL) cos (am->pll.phs), (REAL) sin (am->pll.phs));
REAL diff;
am->pll.delay.re = z.re * sig.re + z.im * sig.im;
am->pll.delay.im = -z.im * sig.re + z.re * sig.im;
diff = fast_atan2 (am->pll.delay.im, am->pll.delay.re);
am->pll.freq.f += am->pll.beta * diff;
if (am->pll.freq.f < am->pll.freq.l)
am->pll.freq.f = am->pll.freq.l;
if (am->pll.freq.f > am->pll.freq.h)
am->pll.freq.f = am->pll.freq.h;
am->pll.phs += am->pll.freq.f + am->pll.alpha * diff;
while (am->pll.phs >= TWOPI)
am->pll.phs -= (REAL) TWOPI;
while (am->pll.phs < 0)
am->pll.phs += (REAL) TWOPI;
}
int polar_disc_fast(int ar, int aj, int br, int bj)
{
int cr, cj;
multiply(ar, aj, br, -bj, &cr, &cj);
return fast_atan2(cj, cr);
}
void complimentary_filter_predict_rad(int16_vect3 accel_raw, uint16_vect3 gyro_raw, int16_vect3 mag_raw, float_vect3* attitude, float_vect3* att_rates){
static float phi_acc_state = 0;
static float theta_acc_state = 0;
static float psi_mag_state = 0;
static float phi_gyro_state = 0;
static float theta_gyro_state = 0;
static float psi_gyro_state = 0;
static float phi_rate_state = 0;
static float theta_rate_state = 0;
static float psi_rate_state = 0;
float phi;
float theta;
float psi;
float phi_acc;
float theta_acc;
float psi_mag;
float phi_gyro;
float theta_gyro;
float psi_gyro;
static float psi_prev = 0;
// static float phi_rate_array[LENGTH_RATE_LOWPASS];
// static float theta_rate_array[LENGTH_RATE_LOWPASS];
// static float psi_rate_array[LENGTH_RATE_LOWPASS];
// static float phi_rate = 0;
// static float theta_rate = 0;
// static float psi_rate = 0;
// static uint32_t rate_idx = 0;
//unbias magnet sensor data
float_vect3 mag;
mag.x = (mag_raw.x-MAG_NEUTRALX)*MAG_SCALEX;
mag.y = (mag_raw.y-MAG_NEUTRALY)*MAG_SCALEY;
mag.z = mag_raw.z;
//unbias acccel
int32_vect3 accel;
accel.x = +accel_raw.x - ACCEL_NEUTRALX; //flipping coordinate system from z upward to downward
accel.y = -accel_raw.y + ACCEL_NEUTRALY;
accel.z = -accel_raw.z + ACCEL_NEUTRALZ;
//unbias gyro
int32_vect3 gyro;
gyro.x = gyro_raw.x - GYRO_NEUTRALX;
gyro.y = -gyro_raw.y + GYRO_NEUTRALY;
gyro.z = gyro_raw.z - GYRO_NEUTRALZ;
//scale gyro to rad/sec
float_vect3 att_rates_raw;
att_rates_raw.x = gyro.x * GYRO_SCALE_X;
att_rates_raw.y = gyro.y * GYRO_SCALE_Y;
att_rates_raw.z = gyro.z * GYRO_SCALE_Z;
//lowpass Accelerations (maybe later)
//lowpass magnet sensor data (maybe later)
//Nick Roll measurement build
float_vect3 attitude_tmp;
attitude_tmp.x = fast_atan2(accel.y,accel.z); //euler angle phi fast_atan2
attitude_tmp.y = -fast_atan2(accel.x,accel.z*(lookup_sin(attitude_tmp.x)+lookup_cos(attitude_tmp.x))); //euler angle theta
//lowpass nick roll
phi_acc = C_LOW*phi_acc_state + D_LOW*attitude_tmp.x;
phi_acc_state = A_LOW*phi_acc_state + B_LOW*attitude_tmp.x;
theta_acc = C_LOW*theta_acc_state + D_LOW*attitude_tmp.y;
theta_acc_state = A_LOW*theta_acc_state + B_LOW*attitude_tmp.y;
//integrate and highpass nick roll from gyro
phi_gyro = C_HIGH*phi_gyro_state + D_HIGH*att_rates_raw.x;
phi_gyro_state = A_HIGH*phi_gyro_state + B_HIGH*att_rates_raw.x;
theta_gyro = C_HIGH*theta_gyro_state + D_HIGH*att_rates_raw.y;
theta_gyro_state = A_HIGH*theta_gyro_state + B_HIGH*att_rates_raw.y;
//add up resulting high and lowpassed angles
phi = phi_acc + phi_gyro;
theta = theta_acc + theta_gyro;
//test from tilt compensation algorithm for 2 axis magnetic compass
mag.z=(sin(DIP_ANGLE)+mag.x*sin(theta)-mag.y*cos(theta)*sin(phi))/(cos(theta)*cos(phi));
//Transformation of maget vector to inertial frame and computation of angle in the x-y plane
attitude_tmp.z = fast_atan2(lookup_cos(theta)*mag.x + lookup_sin(phi)*lookup_sin(theta)*mag.y + lookup_cos(phi)*lookup_sin(theta)*mag.z , lookup_cos(phi)*mag.y - lookup_sin(phi)*mag.z);
// //asembly of angle with range [-PI,PI] to a continous function with range [-2*PI,2*PI]
if(psi_prev > PI/2 && attitude_tmp.z < psi_prev - PI) attitude_tmp.z += 2*PI;
else if(psi_prev < -PI/2 && attitude_tmp.z > psi_prev + PI) attitude_tmp.z -= 2*PI;
else if(attitude_tmp.z > PI/2 && psi_prev < attitude_tmp.z - PI) attitude_tmp.z -= 2*PI;
else if(attitude_tmp.z < -PI/2 && psi_prev > attitude_tmp.z +PI) attitude_tmp.z += 2*PI;
//Reset of angle to Zero if it reaches +-2*PI
if (attitude_tmp.z > 2*PI){
attitude_tmp.z -= 2*PI;
psi_mag_state=0;
}
else if(attitude_tmp.z < -2*PI){
attitude_tmp.z += 2*PI;
psi_mag_state=0;
}
psi_prev = attitude_tmp.z;
//lowpass psi_tmp
psi_mag = C_LOW*psi_mag_state + D_LOW*attitude_tmp.z;
psi_mag_state = A_LOW*psi_mag_state + B_LOW*attitude_tmp.z;
//integrate and highpass yaw from gyro
psi_gyro = C_HIGH*psi_gyro_state + D_HIGH*att_rates_raw.z;
psi_gyro_state = A_HIGH*psi_gyro_state + B_HIGH*att_rates_raw.z;
//add up resulting high and lowpassed angles
psi = psi_mag + psi_gyro;
// //Signal trasformation from range [-2*PI,2*PI] to [-PI,PI]
if(psi > PI) psi = psi -2*PI;
else if(psi < -PI) psi = psi +2*PI;
//output euler angles
(*attitude).x = phi;
(*attitude).y = theta;
(*attitude).z = psi;
//att_rates filtering for gyro rates output in SI units
//lowpass version (0.1sec delay)
(*att_rates).x= C_LOW*phi_rate_state + D_LOW*att_rates_raw.x;
phi_rate_state = A_LOW*phi_rate_state + B_LOW*att_rates_raw.x;
(*att_rates).y= C_LOW*theta_rate_state + D_LOW*att_rates_raw.y;
theta_rate_state = A_LOW*theta_rate_state + B_LOW*att_rates_raw.y;
(*att_rates).z= C_LOW*psi_rate_state + D_LOW*att_rates_raw.z;
psi_rate_state = A_LOW*psi_rate_state + B_LOW*att_rates_raw.z;
// (*att_rates).z= att_rates_raw.z;
// threshold values < |0.008|
// if (((*att_rates).z < 0.008) && (*att_rates).z > - 0.008){
// (*att_rates).z = 0;
// }
// //att_rates filtering for gyro rates output in SI units
// //moving average on gyros 0.05sec delay
// phi_rate -= phi_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// theta_rate -= theta_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// psi_rate -= psi_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
//
// phi_rate_array[rate_idx] = gyro.x;
// theta_rate_array[rate_idx] = gyro.y;
// psi_rate_array[rate_idx] = gyro.z;
//
// phi_rate += phi_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// theta_rate += theta_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// psi_rate += psi_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// ++rate_idx;
// if(rate_idx>=LENGTH_RATE_LOWPASS) rate_idx=0;
//
// (*att_rates).x = phi_rate;
// (*att_rates).y = theta_rate;
// (*att_rates).z = psi_rate;
}
float ABMath::fast_atanv(Vector2 point) {
return fast_atan2(point.y, point.x);
}
