END_TEST
START_TEST(test_vector_three) {
u32 i, t;
seed_rng();
for (t = 0; t < LINALG_NUM; t++) {
double A[3], B[3], C[3], tmp[3];
double D, E, F, norm;
for (i = 0; i < 3; i++) {
A[i] = mrand / 1e20;
B[i] = mrand / 1e20;
C[i] = mrand / 1e20;
}
/* Check triple product identity */
vector_cross(A, B, tmp);
D = vector_dot(3, C, tmp);
vector_cross(B, C, tmp);
E = vector_dot(3, A, tmp);
vector_cross(C, A, tmp);
F = vector_dot(3, B, tmp);
norm = (vector_norm(3, A) + vector_norm(3, B) + vector_norm(3, C))/3;
fail_unless(fabs(E - D) < LINALG_TOL * norm,
"Triple product failure between %lf and %lf",
D, E);
fail_unless(fabs(E - F) < LINALG_TOL * norm,
"Triple product failure between %lf and %lf",
E, F);
fail_unless(fabs(F - D) < LINALG_TOL * norm,
"Triple product failure between %lf and %lf",
F, D);
}
}
/** Simulates real observations for the current position and the satellite almanac and week
* given in simulator_data.
*
* NOTES:
*
* - This simulates the pseudorange as the true distance to the satellite + noise.
* - This simulates the carrier phase as the true distance in wavelengths + bais + noise.
* - The bias is an integer multiple of 10 for easy debugging.
* - The satellite SNR/CN0 is proportional to the elevation of the satellite.
*
* USES:
* - Pipe observations into internals for testing
* - For integration testing with other devices that has to carry the radio signal.
*
* \param elapsed Number of seconds elapsed since last simulation step.
*/
void simulation_step_tracking_and_observations(double elapsed)
{
(void)elapsed;
u8 week = -1;
double t = sim_state.noisy_solution.time.tow;
/* First we calculate all the current sat positions, velocities */
for (u8 i=0; i<simulation_num_almanacs; i++) {
calc_sat_state_almanac(&simulation_almanacs[i], t, week,
simulation_sats_pos[i], simulation_sats_vel[i]);
}
/* Calculate the first sim_settings.num_sats amount of visible sats */
u8 num_sats_selected = 0;
double az, el;
for (u8 i=0; i<simulation_num_almanacs; i++) {
calc_sat_az_el_almanac(&simulation_almanacs[i], t, week,
sim_state.pos, &az, &el);
if (el > 0 &&
num_sats_selected < sim_settings.num_sats &&
num_sats_selected < MAX_CHANNELS) {
/* Generate a code measurement which is just the pseudorange: */
double points_to_sat[3];
double base_points_to_sat[3];
vector_subtract(3, simulation_sats_pos[i], sim_state.pos, points_to_sat);
vector_subtract(3, simulation_sats_pos[i], sim_settings.base_ecef, base_points_to_sat);
double distance_to_sat = vector_norm(3, points_to_sat);
double base_distance_to_sat = vector_norm(3, base_points_to_sat);
/* Fill out the observation details into the NAV_MEAS structure for this satellite, */
/* We simulate the pseudorange as a noisy range measurement, and */
/* the carrier phase as a noisy range in wavelengths + an integer offset. */
populate_nav_meas(&sim_state.nav_meas[num_sats_selected],
distance_to_sat, el, i);
populate_nav_meas(&sim_state.base_nav_meas[num_sats_selected],
base_distance_to_sat, el, i);
/* As for tracking, we just set each sat consecutively in each channel. */
/* This will cause weird jumps when a satellite rises or sets. */
sim_state.tracking_channel[num_sats_selected].state = TRACKING_RUNNING;
sim_state.tracking_channel[num_sats_selected].prn = simulation_almanacs[i].prn + SIM_PRN_OFFSET;
sim_state.tracking_channel[num_sats_selected].cn0 = sim_state.nav_meas[num_sats_selected].snr;
num_sats_selected++;
}
}
sim_state.noisy_solution.n_used = num_sats_selected;
}
vector3d ahrs_drift_correction(dataexchange_t *data) {
vector3d corr_vector, acc_g;
corr_vector.x = 0.0;
corr_vector.y = 0.0;
corr_vector.z = 0.0;
//do correction only then acceleration is close to 1G (unreliable if greater)
acc_g = adxl345_raw_to_g(data, SCALE_2G_10B);
double acc_magnitude = vector_magnitude(acc_g);
if (fabs(acc_magnitude - 1.0) <= 0.15) {
float corr_strength = 0.15;
//vectors for rotation matrix
vector3d acc_v, mag_v;
acc_v.x = (*data).acc_x;
acc_v.y = (*data).acc_y;
acc_v.z = (*data).acc_z;
mag_v.x = (*data).mag_x;
mag_v.y = (*data).mag_y;
mag_v.z = (*data).mag_z;
hmc5883l_applyCalibration(&mag_v, calib_ptr);
vector3d down = vector_inv(acc_v);
vector3d east = vector_cross(down, mag_v);
vector3d north = vector_cross(east, down);
//normalize vectors
vector_norm(&down);
vector_norm(&east);
vector_norm(&north);
//matrix from rotation quaternion
matrix3x3d rot_matrix = quaternion_to_matrix((*data).qr);
//correction vector calculation
vector3d sum1 = vector_sum(vector_cross(north, matrix_row_to_vector(&rot_matrix, 1)),
vector_cross(east, matrix_row_to_vector(&rot_matrix, 2)));
vector3d sum2 = vector_sum(sum1, vector_cross(down, matrix_row_to_vector(&rot_matrix, 3)));
corr_vector = vector_scale(sum2, corr_strength);
}
return corr_vector;
}
vec3 vector_normalized(vec3 v) {
float norm = vector_norm(v);
v.x /= norm;
v.y /= norm;
v.z /= norm;
return v;
}
/*
* Calculate light color
*/
color lightColor(const intersection *inter, const ray *srcRay) {
vec3 v = vector_normalized(vector_float_mul(-1.0f, srcRay->dir));
color cD = BLACK, refCoef = reflectCoef(inter->mat.reflect_coef, inter->normal, srcRay->dir);
// For each light calculate and sum color
for (int k = 0; k < num_lights; ++k) {
vec3 l = vector_minus(lights[k].position, inter->position);
float normL = vector_norm(l);
l = vector_normalized(l);
// Look for object between light source and current intersection
ray objectRay; // Ray from object to light
ray_init(&objectRay, inter->position, l, EPSILON, normL, 0);
float interFound = 0.0f;
for (int i = 0; i < object_count; ++i) {
interFound += interDist(&objectRay, &(scene[i]));
}
if (!interFound) {
vec3 h = vector_normalized(vector_add(l, v));
float sc_nl = clamp(vector_dot(inter->normal, l), 0.0f, 1.0f);
float sc_hn = clamp(vector_dot(h, inter->normal), 0.0f, 1.0f);
cD = vector_add(cD,
vector_vec_mul(lights[k].col,
vector_float_mul(sc_nl,
(vector_add(vector_float_mul(1 / M_PI, inter->mat.kd),
(vector_float_mul(pow(sc_hn, inter->mat.shininess), vector_float_mul(((inter->mat.shininess + 8) / (8 * M_PI)), refCoef))))))));
}
}
return cD;
}
void raytrace(t_env *env)
{
int x;
int y;
t_ray ray;
t_col col;
y = 0;
while (y <= WIN_Y)
{
x = 0;
while (x <= WIN_X)
{
ray.start = OBJ.cam_s;
ray.start.x += x;
ray.start.y += y;
ray.dir = OBJ.cam_dir;
vector_norm(&ray.dir);
set_col(&OBJ.col, 0, 0, 0);
env->br = 0;
col = shoot_ray(ray, 10, env);
save_to_img(env, col, x, y);
x++;
}
y++;
}
}
struct vector* vector_normalize(struct vector* v) {
struct vector* vnorm = vector_new(v->length);
double norm = vector_norm(v);
assert(norm != 0);
vector_normalize_into(vnorm, v);
return vnorm;
}
void vector_normalize_into(struct vector* reciever, struct vector* v) {
double norm = vector_norm(v);
assert(norm != 0);
for(int i = 0; i < v->length; i++) {
VECTOR_IDX_INTO(reciever, i) = VECTOR_IDX_INTO(v, i) / norm;
}
}
void sgp_get_acceleration(vector v_g)// only j2 perturbations taken
{
vector v_r_ecef, v_g_ecef;
float R, R2, R3, R4;
//eci2ecef(v_r, v_r_ecef);//see change
R = vector_norm(r_ecef_ash); //
R2 = pow(R, 2);
R2 = (1.5 * J2 * R_E2) / R2;
R3 = pow(R, 3);
R4 = pow(R, 4);
R4 = (7.5 * J2 * pow(r_ecef_ash[2],2) * R_E2) / R4; //
v_g_ecef[0] = (-1 * GM * r_ecef_ash[0] * (1 + R2 - R4)) / R3;//
v_g_ecef[1] = (-1 * GM * r_ecef_ash[1] * (1 + R2 - R4)) / R3;//
v_g_ecef[2] = (-1 * GM * r_ecef_ash[2] * (1 + 3 * R2 - R4)) / R3;//
ecef2eci(v_g_ecef, v_g);
/* uint8_t sent[3];
for (int i=0;i<2;i=i+1)
{
sent[i] = (uint8_t)((v_g_ecef[i]));
transmit_UART0(sent[i]);
}*/
}
int calculate_distance_map (trajectory_frame *frame, void *p) {
unsigned int i, j;
unsigned int *nbodies = (unsigned int *) p;
distance_map *map = distance_map_alloc (*nbodies);
/* assign distance map to frame data */
frame->data = map;
/* calculate the distance map */
for (i=0; i<*nbodies; i++) {
for (j=i+1; j<*nbodies; j++) {
/* calculate the distance vector */
t_vector diff;
vector_diff (diff,
frame->body_i_state [i].r,
frame->body_i_state [j].r);
/* set the distance map */
set_distance_ij (i, j, vector_norm (diff), map);
}
}
/* print distance map for each frame */
print_distance_map (map);
return LOAD_TRAJ_FRAME_SUCCESS;
}
static void fine_fun(double const* x, double* s)
{
double coarse = 0.5;
double fine = 0.2;
double radius = vector_norm(x, 3);
double d = fabs(radius - 0.5);
s[0] = coarse * d + fine * (1 - d);
}
static void dye_fun(double const coords[3], double* v)
{
double x[3];
double const l[3] = {.25, .5, 0};
double const r[3] = {.75, .5, 0};
double dir = 1;
subtract_vectors(coords, l, x, 3);
if (vector_norm(x, 3) > .25) {
dir = -1;
subtract_vectors(coords, r, x, 3);
}
if (vector_norm(x, 3) > .25) {
v[0] = 0;
return;
}
v[0] = 4 * dir * (.25 - vector_norm(x, 3));
}
int get_heading (data_t* data) {
// Initialize heading value - it's an integer because it is
// going to eventually be binned into North, South, East, or West
vector north, east, mag_vector, accel_vector, from;
int heading;
// Initialize north and east vectors
vector_init(&north, 0, 0, 0);
vector_init(&east, 0, 0, 0);
// Put magnetometer and accelerometer data into vector format
vector_init(&mag_vector, data->Mx, data->My, data-> Mz);
vector_init(&accel_vector, data->Ax, data->Ay, data-> Az);
// Initialize "from" vector based on carrier board design
vector_init(&from, 1, 0, 0);
// Subtract calibration offset from magnetometer readings
mag_vector.x -= ((float) MAG_MAX_X + MAG_MIN_X) / 2.0;
mag_vector.y -= ((float) MAG_MAX_Y + MAG_MIN_Y) / 2.0;
mag_vector.z -= ((float) MAG_MAX_Z + MAG_MIN_Z) / 2.0;
// Compute East and North
vector_cross(&mag_vector, &accel_vector, &east);
vector_norm(&east);
vector_cross(&accel_vector, &east, &north);
vector_norm(&north);
// Compute the heading and make sure it's positive
heading = (int)(atan2(vector_dot(&east, &from), vector_dot(&north, &from))* 180 / PI);
if (heading < 0) {
heading += 360;
}
// Memory cleanup
vector_destroy(&north);
vector_destroy(&east);
vector_destroy(&mag_vector);
vector_destroy(&accel_vector);
vector_destroy(&from);
return heading;
}
END_TEST
START_TEST(test_vector_normalize) {
u32 i, t;
seed_rng();
for (t = 0; t < LINALG_NUM; t++) {
u32 n = sizerand(MSIZE_MAX);
double A[n];
for (i = 0; i < n; i++)
A[i] = mrand;
vector_normalize(n, A);
double vnorm = vector_norm(n, A);
fail_unless(fabs(vnorm - 1) < LINALG_TOL,
"Norm differs from 1: %lf",
vector_norm(n, A));
}
}
static void system_monitor_thread(void *arg)
{
(void)arg;
chRegSetThreadName("system monitor");
systime_t time = chVTGetSystemTime();
bool ant_status = 0;
while (TRUE) {
if (ant_status != frontend_ant_status()) {
ant_status = frontend_ant_status();
if (ant_status && frontend_ant_setting() == AUTO) {
log_info("Now using external antenna.");
}
else if (frontend_ant_setting() == AUTO) {
log_info("Now using patch antenna.");
}
}
u32 status_flags = ant_status << 31 | SBP_MAJOR_VERSION << 16 | SBP_MINOR_VERSION << 8;
sbp_send_msg(SBP_MSG_HEARTBEAT, sizeof(status_flags), (u8 *)&status_flags);
/* If we are in base station mode then broadcast our known location. */
if (broadcast_surveyed_position && position_quality == POSITION_FIX) {
double tmp[3];
double base_ecef[3];
double base_distance;
llhdeg2rad(base_llh, tmp);
wgsllh2ecef(tmp, base_ecef);
vector_subtract(3, base_ecef, position_solution.pos_ecef, tmp);
base_distance = vector_norm(3, tmp);
if (base_distance > BASE_STATION_DISTANCE_THRESHOLD) {
log_warn("Invalid surveyed position coordinates\n");
} else {
sbp_send_msg(SBP_MSG_BASE_POS_ECEF, sizeof(msg_base_pos_ecef_t), (u8 *)&base_ecef);
}
}
msg_iar_state_t iar_state;
if (simulation_enabled_for(SIMULATION_MODE_RTK)) {
iar_state.num_hyps = 1;
} else {
iar_state.num_hyps = dgnss_iar_num_hyps();
}
sbp_send_msg(SBP_MSG_IAR_STATE, sizeof(msg_iar_state_t), (u8 *)&iar_state);
DO_EVERY(2,
send_thread_states();
);
sleep_until(&time, MS2ST(heartbeat_period_milliseconds));
}
float distance1( const Point * A , const Point * B , const Point * C )
{
Vector v1 , v2 , v3 , v4;
vector_from_points( &v1 , A , B );
if ( 0.0f == vector_norm( &v1 , &v1 ) ) return -1.0f;
vector_from_points( &v2 , A , C );
vector_mul( &v3 , &v1 , vector_dot( &v1 , &v2 ) );
vector_sub( &v4 , &v3 , &v2 );
return vector_length( &v4 );
}
quaternion ahrs_orientation_from_accel_mag(dataexchange_t *data) {
//vectors for rotation matrix
vector3d acc_v, mag_v;
matrix3x3d rmat;
quaternion q;
acc_v.x = (*data).acc_x;
acc_v.y = (*data).acc_y;
acc_v.z = (*data).acc_z;
mag_v.x = (*data).mag_x;
mag_v.y = (*data).mag_y;
mag_v.z = (*data).mag_z;
hmc5883l_applyCalibration(&mag_v, calib_ptr);
vector3d down = vector_inv(acc_v);
vector3d east = vector_cross(down, mag_v);
vector3d north = vector_cross(east, down);
//normalize vectors
vector_norm(&down);
vector_norm(&east);
vector_norm(&north);
//vectors to matrix
matrix_vector_to_row(&rmat, north, 1);
matrix_vector_to_row(&rmat, east, 2);
matrix_vector_to_row(&rmat, down, 3);
//matrix to quaternion
q = quaternion_from_matrix(&rmat);
//normalize
quaternion_norm(&q);
(*data).qr = q;
return q;
}
void
gameinfo_collision_ball_slime(GameInfo *gi, TEAM t) {
Ball *b = gi->ball;
Slime *s;
if (t == BLUE ) {
s = gi->slime_blue;
} else {
s = gi->slime_red;
}
Vector v_s = {b->vx, b->vy};
Vector v_a = {s->x - b->x, WIN_HEIGHT-b->y};
Vector v_b = {s->x - (b->x + b->vx), WIN_HEIGHT - (b->y + b->vy)};
double c = vector_inner_product (v_a, v_s) *
vector_inner_product (v_b, v_s) ;
double d = fabs(vector_outer_product (v_s, v_a))/ vector_norm(v_s);
double r = SLIME_RADIUS + BALL_RADIUS;
if ( d <= r && ( c<=0 || r > vector_norm (v_a) || r > vector_norm (v_b))) {
double dx_0 = b->x - s->x;
double dy_0 = b->y - WIN_HEIGHT;
double dist_0 = sqrt(pow(dx_0, 2) + pow(dy_0, 2));
double angle = acos (dx_0/dist_0);
double cos2x = cos(2*angle);
double sin2x = sin(2*angle);
double vx = - b->vx * cos2x + b->vy * sin2x;
double vy = b->vx * sin2x + b->vy * cos2x;
b->vx = vx;
b->vy = vy;
#if 0
double pe = GRAVITY * (WIN_HEIGHT - b->y );
double ke = 0.5 * (pow(b->vx, 2) + pow(b->vy, 2));
pe = GRAVITY * (WIN_HEIGHT - b->y );
ke = 0.5 * (pow(b->vx, 2) + pow(b->vy, 2));
g_print("POST x=%+.2f vx=%+.2f y=%+.2f vy=%+.2f pe=%+.4f ke=%+.4f pe+ke=%+.4f\n",
b->x, b->vx, b->y, b->vy, pe, ke, pe+ke);
#endif
gi->ball_owner = t;
gi->ball_count++;
}
}
/** Approximate chi square test
* Scales least squares residuals by estimated variance,
* compares against threshold.
*
* \param threshold The test threshold.
* Value 5.5 recommended for float/fixed baseline residuals.
* \param num_dds Length of residuals.
* \param residuals Residual vector calculated by lesq_solution_float.
* \param residual If not null, used to output double value of scaled residual.
*
* \return true if norm is below threshold.
*/
static bool chi_test(double threshold, u8 num_dds,
const double *residuals, double *residual)
{
assert(num_dds < MAX_CHANNELS);
double sigma = DEFAULT_PHASE_VAR_KF;
double norm = vector_norm(num_dds, residuals) / sqrt(sigma);
if (residual) {
*residual = norm;
}
return norm < threshold;
}
t_vec vector_cross_product(t_vec u, t_vec v)
{
t_vec result;
double norm;
result.x = v.z * u.y - v.y * u.z;
result.y = v.x * u.z - v.z * u.x;
result.z = v.y * u.x - v.x * u.y;
norm = vector_norm(result);
if (norm != 0)
result = vector_scalar_mult(result, 1 / norm);
return (result);
}
void
vector_normalize(d3_t *v)
{
double len;
if (v != NULL)
{
len = vector_norm(v);
v->x /= len;
v->y /= len;
v->z /= len;
}
}
void rWeaponRaybeam::animate(float spf) {
triggereded = triggered;
if (trigger) fire();
trigger = false;
timeReloading -= spf;
if (timeReloading < 0) {
timeReloading = 0.0f;
if (remainingAmmo == 0 && remainingClips != 0) {
remainingAmmo = clipSize;
if (remainingClips > 0) remainingClips--;
}
}
timeFiring -= spf;
if (timeFiring < 0) {
timeFiring = 0.0f;
return;
}
float* source = weaponPosef;
// Vector that points to the Mountpoint relative to the eye.
float* mpnt = &source[12];
//vector_print(mpnt);
// Mountpoint-Forward Vector relative to the eye.
float* source_8 = &source[8];
float mfwd[3];
vector_cpy(mfwd, source_8);
//vector_print(mfwd);
vector_norm(mfwd, mfwd);
// Interpolate single points towards the "tip" of the ray.
float offset = 80 * (1.0f - (timeFiring / 0.7));
float worldpos[] = {0, 0, 0};
vector_scale(worldpos, mfwd, -offset);
vector_add(worldpos, worldpos, mpnt);
//vector_print(worldpos);
// Collide and Damage with single point on ray.
{
float radius = 0.5;
int roles = 0;
float damage = 25;
int damaged = damageByParticle(worldpos, radius, roles, damage);
// Stop ray on collision.
if (damaged > 0) timeFiring = 0;
}
}
double predict_range(double rx_pos[3],
gps_time_t tot,
ephemeris_t *ephemeris)
{
double sat_pos[3];
double sat_vel[3];
double temp[3];
double clock_err, clock_rate_err;
calc_sat_pos(sat_pos, sat_vel, &clock_err, &clock_rate_err, ephemeris, tot);
vector_subtract(3, sat_pos, rx_pos, temp); // temp = sat_pos - rx_pos
return vector_norm(3, temp);
}
libquat_err unit_vec(const vec_3d *a, vec_3d *n)
{
float norm;
vector_norm(a, &norm);
if (fabsf(norm) < LIBQUAT_EPSILON)
return LIBQUAT_DIV_BY_ZERO;
if (norm != 1.F) // perform division only if needed
{
n->x = a->x / norm;
n->y = a->y / norm;
n->z = a->z / norm;
}
return LIBQUAT_OK;
}
ADPMode* translational_mode(ChemicalUnit* unit,int direction,sym_mat3<double> metrical_matrix) {
vector<Atom*> atoms = unit->get_atoms();
vector<Atom*>::iterator atom;
vec3<double> r(0,0,0);
r[direction]=1;
r=r/vector_norm(r,metrical_matrix);
ADPMode* mode = new ADPMode();
for(atom=atoms.begin(); atom!=atoms.end(); atom++)
mode->add_atom(*atom,r);
return mode;
}
/** Determine the azimuth and elevation of a point in WGS84 Earth Centered,
* Earth Fixed (ECEF) Cartesian coordinates from a reference point given in
* WGS84 ECEF coordinates.
*
* First the vector between the points is converted into the local North, East,
* Down frame of the reference point. Then we can directly calculate the
* azimuth and elevation.
*
* \param ecef Cartesian coordinates of the point, passed as [X, Y, Z],
* all in meters.
* \param ref_ecef Cartesian coordinates of the reference point from which the
* azimuth and elevation is to be determined, passed as
* [X, Y, Z], all in meters.
* \param azimuth Pointer to where to store the calculated azimuth output.
* \param elevation Pointer to where to store the calculated elevation output.
*/
void wgsecef2azel(const double ecef[3], const double ref_ecef[3],
double* azimuth, double* elevation) {
double ned[3];
/* Calculate the vector from the reference point in the local North, East,
* Down frame of the reference point. */
wgsecef2ned_d(ecef, ref_ecef, ned);
*azimuth = atan2(ned[1], ned[0]);
/* atan2 returns angle in range [-pi, pi], usually azimuth is defined in the
* range [0, 2pi]. */
if (*azimuth < 0)
*azimuth += 2*M_PI;
*elevation = asin(-ned[2]/vector_norm(3, ned));
}
ADPMode* rot_mode(ChemicalUnit* unit,vec3<double> axis , vec3<double> point_on_axis ,sym_mat3<double> metrical_matrix) {
vector<Atom*> atoms = unit->get_atoms();
vector<Atom*>::iterator atom;
axis = axis/vector_norm(axis, metrical_matrix.inverse());
ADPMode* mode = new ADPMode();
/*
mat3<double> rotation(1/sqrt(3),-2/sqrt(3),0,2/sqrt(3),-1/sqrt(3),0,0,0,0);
*/
for(atom=atoms.begin(); atom!=atoms.end(); atom++)
mode->add_atom(*atom,metrical_matrix*(axis.cross((*atom)->r - point_on_axis))/sqrt(metrical_matrix.determinant()));
return mode;
}
static void warp_fun(double const* coords, double* v)
{
double x[3];
double const mid[3] = {.5, .5, 0};
subtract_vectors(coords, mid, x, 3);
double polar_a = atan2(x[1], x[0]);
double polar_r = vector_norm(x, 3);
double rot_a = 0;
if (polar_r < .5)
rot_a = the_rotation * (2 * (.5 - polar_r));
double dest_a = polar_a + rot_a;
double dst[3];
dst[0] = cos(dest_a) * polar_r;
dst[1] = sin(dest_a) * polar_r;
dst[2] = 0;
subtract_vectors(dst, x, v, 3);
}
/** Checks pvt_iter residuals.
*
* \param n_used length of omp
* \param omp residual vector calculated by pvt_solve
* \param rx_state reference to pvt solver state allocated in calc_PVT
* \param residual If not null, used to output double value of residual
*
* \return residual < PVT_RESIDUAL_THRESHOLD
*/
static bool residual_test(u8 n_used, double omp[n_used],
const double rx_state[],
double *residual)
{
/* Very liberal threshold. Typical range 20 - 120 */
#define PVT_RESIDUAL_THRESHOLD 3000
/* Need to add clock offset to observed-minus-predicted calculated by last
* iteration of pvt_solve before computing residual. */
for (int i = 0; i < n_used; i++) {
omp[i] -= rx_state[3];
}
double norm = vector_norm(n_used, omp);
if (residual) {
*residual = norm;
}
return norm < PVT_RESIDUAL_THRESHOLD;
}
/** Calculate the Doppler shift of a satellite as observed at a reference
* position given the satellite almanac.
*
* \param alm Pointer to an almanac structure for the satellite of interest.
* \param t GPS time of week at which to calculate the Doppler shift.
* \param week GPS week number modulo 1024 or pass -1 to assume within one
* half-week of the almanac time of applicability.
* \param ref ECEF coordinates of the reference point from which the azimuth
* and elevation is to be determined, passed as [X, Y, Z], all in
* meters.
* \return The Doppler shift in Hz.
*/
double calc_sat_doppler_almanac(almanac_t* alm, double t, s16 week,
double ref[3])
{
double sat_pos[3];
double sat_vel[3];
double vec_ref_sat[3];
calc_sat_state_almanac(alm, t, week, sat_pos, sat_vel);
/* Find the vector from the reference position to the satellite. */
vector_subtract(3, sat_pos, ref, vec_ref_sat);
/* Find the satellite velocity projected on the line of sight vector from the
* reference position to the satellite. */
double radial_velocity = vector_dot(3, vec_ref_sat, sat_vel) / \
vector_norm(3, vec_ref_sat);
/* Return the Doppler shift. */
return GPS_L1_HZ * radial_velocity / NAV_C;
}
