inline void set_ellipse_axes(Point2 &semimajor, Point2 &semiminor, float alpha, float eccentricity, float angle) { semimajor.x = alpha; semimajor.y = 0.f; semiminor.x = 0.f; semiminor.y = semimajor.x * sqrtf(1.f-eccentricity*eccentricity); // rotate ellipse rotate_2D(semimajor, angle); rotate_2D(semiminor, angle); }
/* shift vector to origin, rotate in yz plane and shift back */ Vec v_shift_rotate_yz(Vec *v1, Vec *shift, float phi) { Vec v2; v_dif(&v2, v1, shift); rotate_2D(&(v2.y), &(v2.z), phi); v_sum(&v2, shift, &v2); return v2; }
void estLocation(void) { static int8_t cog_previous = 64; static int16_t sog_previous = 0; static int16_t climb_rate_previous = 0; static uint16_t velocity_previous = 0; static int16_t location_previous[] = { 0, 0, 0 }; union longbbbb accum; union longww accum_velocity; int8_t cog_circular; int8_t cog_delta; int16_t sog_delta; int16_t climb_rate_delta; #ifdef USE_EXTENDED_NAV int32_t location[3]; #else int16_t location[3]; #endif // USE_EXTENDED_NAV int16_t location_deltaZ; struct relative2D location_deltaXY; struct relative2D velocity_thru_air; int16_t velocity_thru_airz; location_plane(&location[0]); // convert GPS course of 360 degrees to a binary model with 256 accum.WW = __builtin_muluu(COURSEDEG_2_BYTECIR, cog_gps.BB) + 0x00008000; // re-orientate from compass (clockwise) to maths (anti-clockwise) with 0 degrees in East cog_circular = -accum.__.B2 + 64; // compensate for GPS reporting latency. // The dynamic model of the EM406 and uBlox is not well known. // However, it seems likely much of it is simply reporting latency. // This section of the code compensates for reporting latency. // markw: what is the latency? It doesn't appear numerically or as a comment // in the following code. Since this method is called at the GPS reporting rate // it must be assumed to be one reporting interval? if (dcm_flags._.gps_history_valid) { cog_delta = cog_circular - cog_previous; sog_delta = sog_gps.BB - sog_previous; climb_rate_delta = climb_gps.BB - climb_rate_previous; location_deltaXY.x = location[0] - location_previous[0]; location_deltaXY.y = location[1] - location_previous[1]; location_deltaZ = location[2] - location_previous[2]; } else { cog_delta = 0; sog_delta = 0; climb_rate_delta = 0; location_deltaXY.x = location_deltaXY.y = location_deltaZ = 0; } dcm_flags._.gps_history_valid = 1; actual_dir = cog_circular + cog_delta; cog_previous = cog_circular; // Note that all these velocities are in centimeters / second ground_velocity_magnitudeXY = sog_gps.BB + sog_delta; sog_previous = sog_gps.BB; GPSvelocity.z = climb_gps.BB + climb_rate_delta; climb_rate_previous = climb_gps.BB; accum_velocity.WW = (__builtin_mulss(cosine(actual_dir), ground_velocity_magnitudeXY) << 2) + 0x00008000; GPSvelocity.x = accum_velocity._.W1; accum_velocity.WW = (__builtin_mulss(sine(actual_dir), ground_velocity_magnitudeXY) << 2) + 0x00008000; GPSvelocity.y = accum_velocity._.W1; rotate_2D(&location_deltaXY, cog_delta); // this is a key step to account for rotation effects!! GPSlocation.x = location[0] + location_deltaXY.x; GPSlocation.y = location[1] + location_deltaXY.y; GPSlocation.z = location[2] + location_deltaZ; location_previous[0] = location[0]; location_previous[1] = location[1]; location_previous[2] = location[2]; velocity_thru_air.y = GPSvelocity.y - estimatedWind[1]; velocity_thru_air.x = GPSvelocity.x - estimatedWind[0]; velocity_thru_airz = GPSvelocity.z - estimatedWind[2]; #if (HILSIM == 1) air_speed_3DGPS = hilsim_airspeed.BB; // use Xplane as a pitot #else air_speed_3DGPS = vector3_mag(velocity_thru_air.x, velocity_thru_air.y, velocity_thru_airz); #endif calculated_heading = rect_to_polar(&velocity_thru_air); // veclocity_thru_air.x becomes XY air speed as a by product of CORDIC routine in rect_to_polar() air_speed_magnitudeXY = velocity_thru_air.x; // in cm / sec #if (GPS_RATE == 4) forward_acceleration = (air_speed_3DGPS - velocity_previous) << 2; // Ublox enters code 4 times per second #elif (GPS_RATE == 2) forward_acceleration = (air_speed_3DGPS - velocity_previous) << 1; // Ublox enters code 2 times per second #else forward_acceleration = (air_speed_3DGPS - velocity_previous); // EM406 standard GPS enters code once per second #endif velocity_previous = air_speed_3DGPS; }